Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
97  vn100_trans.output_buf = (uint8_t *)&last_send_packet;
98  vn100_trans.input_buf = (uint8_t *)&last_received_packet;
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 :
116  last_send_packet.RegID = VN100_REG_SBAUD;
118  ins_init_status++;
119  break;
120  case INS_VN100_SET_ADOR :
121  last_send_packet.RegID = VN100_REG_ADOR;
123  ins_init_status++;
124  break;
125  case INS_VN100_SET_ADOF :
126  last_send_packet.RegID = VN100_REG_ADOF;
128  ins_init_status++;
129  break;
130  case INS_VN100_READY :
131  return true;
132  }
133  last_send_packet.CmdID = VN100_CmdID_WriteRegister;
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
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 ?
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 {
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;
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;
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;
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;
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;
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;
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;
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"
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 
#define VN100_REG_QTR
Definition: VN100.h:45
float ins_roll_neutral
Definition: ins_vn100.c:53
#define VN100_REG_QAR
Definition: VN100.h:47
static void parse_ins_msg(void)
Definition: ins_vn100.c:185
#define VN100_ADOR
Definition: ins_vn100.h:67
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:149
struct FloatRates ins_rates
Definition: ins_vn100.c:59
void vn100_init(void)
Definition: ins_vn100.c:85
#define VN100_REG_SBAUD
Definition: VN100.h:38
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
#define VN100_REG_YMR_SIZE
Definition: VN100.h:90
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define VN100_REG_SBAUD_SIZE
Definition: VN100.h:68
float phi
in radians
uint8_t ErrID
Definition: VN100.h:191
#define VN100_REG_QMR
Definition: VN100.h:48
uint16_t output_length
number of data words to write
Definition: spi.h:146
#define VN100_REG_YMR
Definition: VN100.h:60
VN100_Param Data[VN100_SPI_BUFFER_SIZE]
Definition: VN100.h:192
CPHA = 1.
Definition: spi.h:69
#define VN100_SPI_DEV
Definition: ins_vn100.c:44
float r
in rad/s
#define VN100_Error_None
Definition: VN100.h:104
#define VN100_REG_QTM
Definition: VN100.h:43
#define VN100_REG_ADOR
Definition: VN100.h:39
#define VN100_REG_ADOF_SIZE
Definition: VN100.h:70
SPI transaction structure.
Definition: spi.h:142
float psi
in radians
uint32_t UInt
Definition: VN100.h:170
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:505
struct spi_transaction vn100_trans
Definition: ins_vn100.c:82
#define VN100_REG_ADOF
Definition: VN100.h:40
VN100_Req_Packet last_send_packet
Definition: ins_vn100.c:69
CPOL = 1.
Definition: spi.h:78
float q
in rad/s
float p
in rad/s
struct FloatVect3 ins_mag
Definition: ins_vn100.c:62
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
uint8_t ins_init_status
Definition: ins_vn100.c:76
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
volatile uint8_t ins_msg_received
Definition: ins_vn100.c:64
float ins_yaw_neutral
Definition: ins_vn100.c:55
euler angles
Roation quaternion.
Architecture independent SPI (Serial Peripheral Interface) API.
void vn100_report_task(void)
Definition: ins_vn100.c:311
uint32_t ins_baud
Definition: ins_vn100.c:74
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:150
#define INS_VN100_SET_ADOF
Definition: ins_vn100.h:80
float theta
in radians
#define TRUE
Definition: std.h:4
void vn100_event_task(void)
Definition: ins_vn100.c:160
Definition: spi.h:84
#define VN100_SLAVE_IDX
Definition: ins_vn100.c:49
struct FloatRMat ins_rmat
Definition: ins_vn100.c:60
uint32_t ins_ador
Definition: ins_vn100.c:72
unsigned long uint32_t
Definition: types.h:18
#define VN100_REG_QTA
Definition: VN100.h:44
uint32_t ins_adof
Definition: ins_vn100.c:73
uint8_t CmdID
Definition: VN100.h:179
void vn100_periodic_task(void)
Definition: ins_vn100.c:140
#define VN100_REG_ADOR_SIZE
Definition: VN100.h:69
#define VN100_REG_QTN
Definition: VN100.h:42
#define INS_YAW_NEUTRAL_DEFAULT
driver for the VectorNav VN100
Definition: ins_vn100.c:39
#define INS_VN100_SET_ADOR
Definition: ins_vn100.h:79
uint16_t input_length
number of data words to read
Definition: spi.h:145
float ins_pitch_neutral
Definition: ins_vn100.c:54
#define INS_VN100_SET_BAUD
Definition: ins_vn100.h:78
#define VN100_BAUD
Definition: ins_vn100.h:73
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define VN100_REG_QMA
Definition: VN100.h:46
slave is selected before transaction and unselected after
Definition: spi.h:57
static bool ins_configure(void)
Definition: ins_vn100.c:109
uint8_t RegID
Definition: VN100.h:190
#define VN100_CmdID_WriteRegister
Definition: VN100.h:97
#define INS_VN100_READY
Definition: ins_vn100.h:81
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:147
rotation matrix
float Float
Definition: VN100.h:171
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
struct FloatEulers ins_eulers
Definition: ins_vn100.c:57
#define VN100_ADOF
Definition: ins_vn100.h:70
Interface for the VectorNav VN100 AHRS use the binary protocal on the SPI link.
#define VN100_REG_YPR
Definition: VN100.h:41
VN100_Res_Packet last_received_packet
Definition: ins_vn100.c:67
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define VN100_CmdID_ReadRegister
Definition: VN100.h:96
struct FloatQuat ins_quat
Definition: ins_vn100.c:58
angular rates
uint8_t RegID
Definition: VN100.h:180
enum SPITransactionStatus status
Definition: spi.h:156
struct FloatVect3 ins_accel
Definition: ins_vn100.c:61