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"
35 #include "subsystems/datalink/downlink.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 */
53 float ins_roll_neutral;
54 float ins_pitch_neutral;
55 float ins_yaw_neutral;
56 
57 struct FloatEulers ins_eulers;
58 struct FloatQuat ins_quat;
59 struct FloatRates ins_rates;
60 struct FloatRMat ins_rmat;
61 struct FloatVect3 ins_accel;
62 struct FloatVect3 ins_mag;
63 
64 volatile uint8_t ins_msg_received;
65 
66 /* last received SPI packet */
67 VN100_Res_Packet last_received_packet;
68 /* last send packet */
69 VN100_Req_Packet last_send_packet;
70 
71 /* output mode */
72 uint32_t ins_ador;
73 uint32_t ins_adof;
74 uint32_t ins_baud;
75 
76 uint8_t ins_init_status;
77 
78 // parsing function
79 static inline void parse_ins_msg(void);
80 
81 /* spi transaction */
82 struct spi_transaction vn100_trans;
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;
90  ins_yaw_neutral = INS_YAW_NEUTRAL_DEFAULT;
91 
92  vn100_trans.slave_idx = VN100_SLAVE_IDX;
93  vn100_trans.cpol = SPICpolIdleHigh;
94  vn100_trans.cpha = SPICphaEdge2;
95  vn100_trans.dss = SPIDss8bit;
96  vn100_trans.select = SPISelectUnselect;
97  vn100_trans.output_buf = (uint8_t *)&last_send_packet;
98  vn100_trans.input_buf = (uint8_t *)&last_received_packet;
99  vn100_trans.status = SPITransDone;
100 
101  ins_ador = VN100_ADOR;
102  ins_adof = VN100_ADOF;
103  ins_baud = VN100_BAUD;
104 
105  ins_init_status = INS_VN100_SET_BAUD;
106 
107 }
108 
109 static inline bool ins_configure(void)
110 {
111  // nothing to receive during conf
112  vn100_trans.input_length = 0;
113 
114  switch (ins_init_status) {
115  case INS_VN100_SET_BAUD :
116  last_send_packet.RegID = VN100_REG_SBAUD;
117  vn100_trans.output_length = 4 + VN100_REG_SBAUD_SIZE;
118  ins_init_status++;
119  break;
120  case INS_VN100_SET_ADOR :
121  last_send_packet.RegID = VN100_REG_ADOR;
122  vn100_trans.output_length = 4 + VN100_REG_ADOR_SIZE;
123  ins_init_status++;
124  break;
125  case INS_VN100_SET_ADOF :
126  last_send_packet.RegID = VN100_REG_ADOF;
127  vn100_trans.output_length = 4 + VN100_REG_ADOF_SIZE;
128  ins_init_status++;
129  break;
130  case INS_VN100_READY :
131  return true;
132  }
133  last_send_packet.CmdID = VN100_CmdID_WriteRegister;
134 
135  spi_submit(&(VN100_SPI_DEV), &vn100_trans);
136 
137  return false;
138 }
139 
140 void vn100_periodic_task(void)
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
149  last_send_packet.CmdID = VN100_CmdID_ReadRegister;
150  last_send_packet.RegID = VN100_REG_YMR;
151  // Set IO length
152  vn100_trans.output_length = 2; // Only 2 ?
153  vn100_trans.input_length = 4 + VN100_REG_YMR_SIZE;
154  // submit
155  spi_submit(&(VN100_SPI_DEV), &vn100_trans);
156  }
157 
158 }
159 
160 void vn100_event_task(void)
161 {
162  if (vn100_trans.status == SPITransSuccess) {
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 = {
168  ins_eulers.phi - ins_roll_neutral,
169  ins_eulers.theta - ins_pitch_neutral,
170  ins_eulers.psi
171  };
172  stateSetNedToBodyEulers_f(&att);
173  stateSetBodyRates_f(&ins_rates);
174 #endif
175  //uint8_t s = 4+VN100_REG_QMR_SIZE;
176  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
177  vn100_trans.status = SPITransDone;
178  }
179  if (vn100_trans.status == SPITransFailed) {
180  vn100_trans.status = SPITransDone;
181  // FIXME retry config if not done ?
182  }
183 }
184 
185 static inline void parse_ins_msg(void)
186 {
187  if (last_received_packet.ErrID != VN100_Error_None) {
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 :
195  ins_ador = last_received_packet.Data[0].UInt;
196  break;
197  case VN100_REG_ADOF :
198  ins_adof = last_received_packet.Data[0].UInt;
199  break;
200  case VN100_REG_SBAUD :
201  ins_baud = last_received_packet.Data[0].UInt;
202  break;
203  case VN100_REG_YPR :
204  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
205  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
206  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
207  break;
208  case VN100_REG_QTN :
209  ins_quat.qi = last_received_packet.Data[0].Float;
210  ins_quat.qx = last_received_packet.Data[1].Float;
211  ins_quat.qy = last_received_packet.Data[2].Float;
212  ins_quat.qz = last_received_packet.Data[3].Float;
213  float_eulers_of_quat(&ins_eulers, &ins_quat);
214  break;
215  case VN100_REG_QTM :
216  ins_quat.qi = last_received_packet.Data[0].Float;
217  ins_quat.qx = last_received_packet.Data[1].Float;
218  ins_quat.qy = last_received_packet.Data[2].Float;
219  ins_quat.qz = last_received_packet.Data[3].Float;
220  float_eulers_of_quat(&ins_eulers, &ins_quat);
221  ins_mag.x = last_received_packet.Data[4].Float;
222  ins_mag.y = last_received_packet.Data[5].Float;
223  ins_mag.z = last_received_packet.Data[6].Float;
224  break;
225  case VN100_REG_QTA :
226  ins_quat.qi = last_received_packet.Data[0].Float;
227  ins_quat.qx = last_received_packet.Data[1].Float;
228  ins_quat.qy = last_received_packet.Data[2].Float;
229  ins_quat.qz = last_received_packet.Data[3].Float;
230  float_eulers_of_quat(&ins_eulers, &ins_quat);
231  ins_accel.x = last_received_packet.Data[4].Float;
232  ins_accel.y = last_received_packet.Data[5].Float;
233  ins_accel.z = last_received_packet.Data[6].Float;
234  break;
235  case VN100_REG_QTR :
236  ins_quat.qi = last_received_packet.Data[0].Float;
237  ins_quat.qx = last_received_packet.Data[1].Float;
238  ins_quat.qy = last_received_packet.Data[2].Float;
239  ins_quat.qz = last_received_packet.Data[3].Float;
240  float_eulers_of_quat(&ins_eulers, &ins_quat);
241  ins_rates.p = last_received_packet.Data[4].Float;
242  ins_rates.q = last_received_packet.Data[5].Float;
243  ins_rates.r = last_received_packet.Data[6].Float;
244  break;
245  case VN100_REG_QMA :
246  ins_quat.qi = last_received_packet.Data[0].Float;
247  ins_quat.qx = last_received_packet.Data[1].Float;
248  ins_quat.qy = last_received_packet.Data[2].Float;
249  ins_quat.qz = last_received_packet.Data[3].Float;
250  float_eulers_of_quat(&ins_eulers, &ins_quat);
251  ins_mag.x = last_received_packet.Data[4].Float;
252  ins_mag.y = last_received_packet.Data[5].Float;
253  ins_mag.z = last_received_packet.Data[6].Float;
254  ins_accel.x = last_received_packet.Data[7].Float;
255  ins_accel.y = last_received_packet.Data[8].Float;
256  ins_accel.z = last_received_packet.Data[9].Float;
257  break;
258  case VN100_REG_QAR :
259  ins_quat.qi = last_received_packet.Data[0].Float;
260  ins_quat.qx = last_received_packet.Data[1].Float;
261  ins_quat.qy = last_received_packet.Data[2].Float;
262  ins_quat.qz = last_received_packet.Data[3].Float;
263  float_eulers_of_quat(&ins_eulers, &ins_quat);
264  ins_accel.x = last_received_packet.Data[4].Float;
265  ins_accel.y = last_received_packet.Data[5].Float;
266  ins_accel.z = last_received_packet.Data[6].Float;
267  ins_rates.p = last_received_packet.Data[7].Float;
268  ins_rates.q = last_received_packet.Data[8].Float;
269  ins_rates.r = last_received_packet.Data[9].Float;
270  break;
271  case VN100_REG_QMR :
272  ins_quat.qi = last_received_packet.Data[0].Float;
273  ins_quat.qx = last_received_packet.Data[1].Float;
274  ins_quat.qy = last_received_packet.Data[2].Float;
275  ins_quat.qz = last_received_packet.Data[3].Float;
276  float_eulers_of_quat(&ins_eulers, &ins_quat);
277  ins_mag.x = last_received_packet.Data[4].Float;
278  ins_mag.y = last_received_packet.Data[5].Float;
279  ins_mag.z = last_received_packet.Data[6].Float;
280  ins_accel.x = last_received_packet.Data[7].Float;
281  ins_accel.y = last_received_packet.Data[8].Float;
282  ins_accel.z = last_received_packet.Data[9].Float;
283  ins_rates.p = last_received_packet.Data[10].Float;
284  ins_rates.q = last_received_packet.Data[11].Float;
285  ins_rates.r = last_received_packet.Data[12].Float;
286  break;
287  case VN100_REG_YMR :
288  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
289  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
290  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
291  ins_mag.x = last_received_packet.Data[3].Float;
292  ins_mag.y = last_received_packet.Data[4].Float;
293  ins_mag.z = last_received_packet.Data[5].Float;
294  ins_accel.x = last_received_packet.Data[6].Float;
295  ins_accel.y = last_received_packet.Data[7].Float;
296  ins_accel.z = last_received_packet.Data[8].Float;
297  ins_rates.p = last_received_packet.Data[9].Float;
298  ins_rates.q = last_received_packet.Data[10].Float;
299  ins_rates.r = last_received_packet.Data[11].Float;
300  break;
301  default:
302  break;
303  }
304 
305 }
306 
307 #include "mcu_periph/uart.h"
308 #include "pprzlink/messages.h"
309 #include "subsystems/datalink/downlink.h"
310 
311 extern void vn100_report_task(void)
312 {
313  DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
314  &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi,
315  &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz,
316  &ins_rates.p, &ins_rates.q, &ins_rates.r,
317  &ins_accel.x, &ins_accel.y, &ins_accel.z,
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