Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ins_vn100.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 ENAC
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
27 #include "modules/ins/ins_vn100.h"
28 
29 #include "generated/airframe.h"
30 #include "mcu_periph/spi.h"
31 #include "state.h"
32 
33 // for telemetry report
34 #include "mcu_periph/uart.h"
36 #include "pprzlink/messages.h"
37 
38 #ifndef INS_YAW_NEUTRAL_DEFAULT
39 #define INS_YAW_NEUTRAL_DEFAULT 0.
40 #endif
41 
42 // default spi device
43 #ifndef VN100_SPI_DEV
44 #define VN100_SPI_DEV spi1
45 #endif
46 
47 // default slave number
48 #ifndef VN100_SLAVE_IDX
49 #define VN100_SLAVE_IDX 0
50 #endif
51 
52 /* neutrals */
56 
63 
65 
66 /* last received SPI packet */
68 /* last send packet */
70 
71 /* output mode */
75 
77 
78 // parsing function
79 static inline void parse_ins_msg(void);
80 
81 /* spi transaction */
83 
84 /* init vn100 */
85 void vn100_init(void)
86 {
87 
88  //ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
89  //ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
91 
100 
104 
106 
107 }
108 
109 static inline bool ins_configure(void)
110 {
111  // nothing to receive during conf
113 
114  switch (ins_init_status) {
115  case INS_VN100_SET_BAUD :
118  ins_init_status++;
119  break;
120  case INS_VN100_SET_ADOR :
123  ins_init_status++;
124  break;
125  case INS_VN100_SET_ADOF :
128  ins_init_status++;
129  break;
130  case INS_VN100_READY :
131  return true;
132  }
134 
136 
137  return false;
138 }
139 
141 {
142 
143  // only send config or request when last transaction is done
144  if (vn100_trans.status != SPITransDone) { return; }
145 
146  // send request when configuration is done
147  if (ins_configure() == TRUE) {
148  // Fill request for QMR
151  // Set IO length
152  vn100_trans.output_length = 2; // Only 2 ?
154  // submit
156  }
157 
158 }
159 
161 {
163  parse_ins_msg();
164 #ifndef INS_VN100_READ_ONLY
165  // Update estimator
166  // FIXME Use a proper rotation matrix here
167  struct FloatEulers att = {
171  };
174 #endif
175  //uint8_t s = 4+VN100_REG_QMR_SIZE;
176  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
178  }
181  // FIXME retry config if not done ?
182  }
183 }
184 
185 static inline void parse_ins_msg(void)
186 {
188  //TODO send error
189  return;
190  }
191 
192  // parse message (will work only with read and write register)
193  switch (last_received_packet.RegID) {
194  case VN100_REG_ADOR :
196  break;
197  case VN100_REG_ADOF :
199  break;
200  case VN100_REG_SBAUD :
202  break;
203  case VN100_REG_YPR :
207  break;
208  case VN100_REG_QTN :
214  break;
215  case VN100_REG_QTM :
224  break;
225  case VN100_REG_QTA :
234  break;
235  case VN100_REG_QTR :
244  break;
245  case VN100_REG_QMA :
257  break;
258  case VN100_REG_QAR :
270  break;
271  case VN100_REG_QMR :
286  break;
287  case VN100_REG_YMR :
300  break;
301  default:
302  break;
303  }
304 
305 }
306 
307 #include "mcu_periph/uart.h"
308 #include "pprzlink/messages.h"
310 
311 extern void vn100_report_task(void)
312 {
313  DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
318  &ins_mag.x, &ins_mag.y, &ins_mag.z);
319 }
320 
ins_mag
struct FloatVect3 ins_mag
Definition: ins_vn100.c:62
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
VN100_REG_YMR_SIZE
#define VN100_REG_YMR_SIZE
Definition: VN100.h:90
ins_rmat
struct FloatRMat ins_rmat
Definition: ins_vn100.c:60
VN100_REG_ADOR_SIZE
#define VN100_REG_ADOR_SIZE
Definition: VN100.h:69
VN100_ADOR
#define VN100_ADOR
Definition: ins_vn100.h:67
spi_transaction::cpol
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:155
ins_baud
uint32_t ins_baud
Definition: ins_vn100.c:74
VN100_Req_Packet::CmdID
uint8_t CmdID
Definition: VN100.h:179
VN100_Req_Packet
Definition: VN100.h:178
VN100_REG_YPR
#define VN100_REG_YPR
Definition: VN100.h:41
VN100_Res_Packet::Data
VN100_Param Data[VN100_SPI_BUFFER_SIZE]
Definition: VN100.h:192
spi_transaction::output_length
uint16_t output_length
number of data words to write
Definition: spi.h:152
VN100_Res_Packet
Definition: VN100.h:187
spi.h
VN100_Error_None
#define VN100_Error_None
Definition: VN100.h:104
INS_VN100_READY
#define INS_VN100_READY
Definition: ins_vn100.h:81
last_received_packet
VN100_Res_Packet last_received_packet
Definition: ins_vn100.c:67
spi_transaction
SPI transaction structure.
Definition: spi.h:148
ins_rates
struct FloatRates ins_rates
Definition: ins_vn100.c:59
VN100_CmdID_ReadRegister
#define VN100_CmdID_ReadRegister
Definition: VN100.h:96
INS_YAW_NEUTRAL_DEFAULT
#define INS_YAW_NEUTRAL_DEFAULT
driver for the VectorNav VN100
Definition: ins_vn100.c:39
uint32_t
unsigned long uint32_t
Definition: types.h:18
vn100_trans
struct spi_transaction vn100_trans
Definition: ins_vn100.c:82
SPISelectUnselect
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition: spi.h:63
SPITransSuccess
@ SPITransSuccess
Definition: spi.h:99
VN100_REG_SBAUD_SIZE
#define VN100_REG_SBAUD_SIZE
Definition: VN100.h:68
VN100_Param::UInt
uint32_t UInt
Definition: VN100.h:170
ins_configure
static bool ins_configure(void)
Definition: ins_vn100.c:109
VN100_REG_QTN
#define VN100_REG_QTN
Definition: VN100.h:42
spi_transaction::output_buf
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
vn100_report_task
void vn100_report_task(void)
Definition: ins_vn100.c:311
VN100_ADOF
#define VN100_ADOF
Definition: ins_vn100.h:70
VN100_Res_Packet::ErrID
uint8_t ErrID
Definition: VN100.h:191
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
spi_transaction::select
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
VN100_REG_YMR
#define VN100_REG_YMR
Definition: VN100.h:60
VN100_REG_QMR
#define VN100_REG_QMR
Definition: VN100.h:48
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
last_send_packet
VN100_Req_Packet last_send_packet
Definition: ins_vn100.c:69
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
spi_transaction::cpha
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:156
spi_submit
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:511
VN100_REG_QAR
#define VN100_REG_QAR
Definition: VN100.h:47
SPITransFailed
@ SPITransFailed
Definition: spi.h:100
VN100_REG_QTM
#define VN100_REG_QTM
Definition: VN100.h:43
VN100_REG_ADOR
#define VN100_REG_ADOR
Definition: VN100.h:39
VN100_REG_SBAUD
#define VN100_REG_SBAUD
Definition: VN100.h:38
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
vn100_event_task
void vn100_event_task(void)
Definition: ins_vn100.c:160
VN100_SLAVE_IDX
#define VN100_SLAVE_IDX
Definition: ins_vn100.c:49
VN100_BAUD
#define VN100_BAUD
Definition: ins_vn100.h:73
VN100_Res_Packet::RegID
uint8_t RegID
Definition: VN100.h:190
VN100_CmdID_WriteRegister
#define VN100_CmdID_WriteRegister
Definition: VN100.h:97
SPICpolIdleHigh
@ SPICpolIdleHigh
CPOL = 1.
Definition: spi.h:84
uint8_t
unsigned char uint8_t
Definition: types.h:14
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
ins_eulers
struct FloatEulers ins_eulers
Definition: ins_vn100.c:57
vn100_init
void vn100_init(void)
Definition: ins_vn100.c:85
SPIDss8bit
@ SPIDss8bit
Definition: spi.h:90
VN100_REG_QTA
#define VN100_REG_QTA
Definition: VN100.h:44
vn100_periodic_task
void vn100_periodic_task(void)
Definition: ins_vn100.c:140
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
ins_pitch_neutral
float ins_pitch_neutral
Definition: ins_vn100.c:54
ins_roll_neutral
float ins_roll_neutral
Definition: ins_vn100.c:53
INS_VN100_SET_ADOF
#define INS_VN100_SET_ADOF
Definition: ins_vn100.h:80
spi_transaction::input_length
uint16_t input_length
number of data words to read
Definition: spi.h:151
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
spi_transaction::dss
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
ins_init_status
uint8_t ins_init_status
Definition: ins_vn100.c:76
spi_transaction::slave_idx
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:153
SPITransDone
@ SPITransDone
Definition: spi.h:101
VN100_REG_ADOF_SIZE
#define VN100_REG_ADOF_SIZE
Definition: VN100.h:70
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
spi_transaction::input_buf
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
ins_vn100.h
Interface for the VectorNav VN100 AHRS use the binary protocal on the SPI link.
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
SPICphaEdge2
@ SPICphaEdge2
CPHA = 1.
Definition: spi.h:75
ins_accel
struct FloatVect3 ins_accel
Definition: ins_vn100.c:61
VN100_Param::Float
float Float
Definition: VN100.h:171
ins_yaw_neutral
float ins_yaw_neutral
Definition: ins_vn100.c:55
parse_ins_msg
static void parse_ins_msg(void)
Definition: ins_vn100.c:185
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
VN100_REG_QMA
#define VN100_REG_QMA
Definition: VN100.h:46
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
VN100_Req_Packet::RegID
uint8_t RegID
Definition: VN100.h:180
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
state.h
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
INS_VN100_SET_ADOR
#define INS_VN100_SET_ADOR
Definition: ins_vn100.h:79
TRUE
#define TRUE
Definition: std.h:4
INS_VN100_SET_BAUD
#define INS_VN100_SET_BAUD
Definition: ins_vn100.h:78
ins_msg_received
volatile uint8_t ins_msg_received
Definition: ins_vn100.c:64
ins_adof
uint32_t ins_adof
Definition: ins_vn100.c:73
ins_ador
uint32_t ins_ador
Definition: ins_vn100.c:72
spi_transaction::status
enum SPITransactionStatus status
Definition: spi.h:162
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
ins_quat
struct FloatQuat ins_quat
Definition: ins_vn100.c:58
VN100_REG_ADOF
#define VN100_REG_ADOF
Definition: VN100.h:40
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
VN100_SPI_DEV
#define VN100_SPI_DEV
Definition: ins_vn100.c:44
VN100_REG_QTR
#define VN100_REG_QTR
Definition: VN100.h:45