Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 "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  //ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
88  //ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
90 
96  vn100_trans.output_buf = (uint8_t*)&last_send_packet;
97  vn100_trans.input_buf = (uint8_t*)&last_received_packet;
99 
103 
105 
106 }
107 
108 static inline bool_t ins_configure( void ) {
109  // nothing to receive during conf
111 
112  switch (ins_init_status) {
113  case INS_VN100_SET_BAUD :
114  last_send_packet.RegID = VN100_REG_SBAUD;
116  ins_init_status++;
117  break;
118  case INS_VN100_SET_ADOR :
119  last_send_packet.RegID = VN100_REG_ADOR;
121  ins_init_status++;
122  break;
123  case INS_VN100_SET_ADOF :
124  last_send_packet.RegID = VN100_REG_ADOF;
126  ins_init_status++;
127  break;
128  case INS_VN100_READY :
129  return TRUE;
130  }
131  last_send_packet.CmdID = VN100_CmdID_WriteRegister;
132 
134 
135  return FALSE;
136 }
137 
138 void vn100_periodic_task( void ) {
139 
140  // only send config or request when last transaction is done
141  if (vn100_trans.status != SPITransDone) { return; }
142 
143  // send request when configuration is done
144  if (ins_configure() == TRUE) {
145  // Fill request for QMR
146  last_send_packet.CmdID = VN100_CmdID_ReadRegister;
147  last_send_packet.RegID = VN100_REG_YMR;
148  // Set IO length
149  vn100_trans.output_length = 2; // Only 2 ?
151  // submit
153  }
154 
155 }
156 
157 void vn100_event_task( void ) {
159  parse_ins_msg();
160 #ifndef INS_VN100_READ_ONLY
161  // Update estimator
162  // FIXME Use a proper rotation matrix here
163  struct FloatEulers att = {
167  };
170 #endif
171  //uint8_t s = 4+VN100_REG_QMR_SIZE;
172  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
174  }
177  // FIXME retry config if not done ?
178  }
179 }
180 
181 static inline void parse_ins_msg( void ) {
182  if (last_received_packet.ErrID != VN100_Error_None) {
183  //TODO send error
184  return;
185  }
186 
187  // parse message (will work only with read and write register)
188  switch (last_received_packet.RegID) {
189  case VN100_REG_ADOR :
190  ins_ador = last_received_packet.Data[0].UInt;
191  break;
192  case VN100_REG_ADOF :
193  ins_adof = last_received_packet.Data[0].UInt;
194  break;
195  case VN100_REG_SBAUD :
196  ins_baud = last_received_packet.Data[0].UInt;
197  break;
198  case VN100_REG_YPR :
199  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
200  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
201  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
202  break;
203  case VN100_REG_QTN :
204  ins_quat.qi = last_received_packet.Data[0].Float;
205  ins_quat.qx = last_received_packet.Data[1].Float;
206  ins_quat.qy = last_received_packet.Data[2].Float;
207  ins_quat.qz = last_received_packet.Data[3].Float;
209  break;
210  case VN100_REG_QTM :
211  ins_quat.qi = last_received_packet.Data[0].Float;
212  ins_quat.qx = last_received_packet.Data[1].Float;
213  ins_quat.qy = last_received_packet.Data[2].Float;
214  ins_quat.qz = last_received_packet.Data[3].Float;
216  ins_mag.x = last_received_packet.Data[4].Float;
217  ins_mag.y = last_received_packet.Data[5].Float;
218  ins_mag.z = last_received_packet.Data[6].Float;
219  break;
220  case VN100_REG_QTA :
221  ins_quat.qi = last_received_packet.Data[0].Float;
222  ins_quat.qx = last_received_packet.Data[1].Float;
223  ins_quat.qy = last_received_packet.Data[2].Float;
224  ins_quat.qz = last_received_packet.Data[3].Float;
226  ins_accel.x = last_received_packet.Data[4].Float;
227  ins_accel.y = last_received_packet.Data[5].Float;
228  ins_accel.z = last_received_packet.Data[6].Float;
229  break;
230  case VN100_REG_QTR :
231  ins_quat.qi = last_received_packet.Data[0].Float;
232  ins_quat.qx = last_received_packet.Data[1].Float;
233  ins_quat.qy = last_received_packet.Data[2].Float;
234  ins_quat.qz = last_received_packet.Data[3].Float;
236  ins_rates.p = last_received_packet.Data[4].Float;
237  ins_rates.q = last_received_packet.Data[5].Float;
238  ins_rates.r = last_received_packet.Data[6].Float;
239  break;
240  case VN100_REG_QMA :
241  ins_quat.qi = last_received_packet.Data[0].Float;
242  ins_quat.qx = last_received_packet.Data[1].Float;
243  ins_quat.qy = last_received_packet.Data[2].Float;
244  ins_quat.qz = last_received_packet.Data[3].Float;
246  ins_mag.x = last_received_packet.Data[4].Float;
247  ins_mag.y = last_received_packet.Data[5].Float;
248  ins_mag.z = last_received_packet.Data[6].Float;
249  ins_accel.x = last_received_packet.Data[7].Float;
250  ins_accel.y = last_received_packet.Data[8].Float;
251  ins_accel.z = last_received_packet.Data[9].Float;
252  break;
253  case VN100_REG_QAR :
254  ins_quat.qi = last_received_packet.Data[0].Float;
255  ins_quat.qx = last_received_packet.Data[1].Float;
256  ins_quat.qy = last_received_packet.Data[2].Float;
257  ins_quat.qz = last_received_packet.Data[3].Float;
259  ins_accel.x = last_received_packet.Data[4].Float;
260  ins_accel.y = last_received_packet.Data[5].Float;
261  ins_accel.z = last_received_packet.Data[6].Float;
262  ins_rates.p = last_received_packet.Data[7].Float;
263  ins_rates.q = last_received_packet.Data[8].Float;
264  ins_rates.r = last_received_packet.Data[9].Float;
265  break;
266  case VN100_REG_QMR :
267  ins_quat.qi = last_received_packet.Data[0].Float;
268  ins_quat.qx = last_received_packet.Data[1].Float;
269  ins_quat.qy = last_received_packet.Data[2].Float;
270  ins_quat.qz = last_received_packet.Data[3].Float;
272  ins_mag.x = last_received_packet.Data[4].Float;
273  ins_mag.y = last_received_packet.Data[5].Float;
274  ins_mag.z = last_received_packet.Data[6].Float;
275  ins_accel.x = last_received_packet.Data[7].Float;
276  ins_accel.y = last_received_packet.Data[8].Float;
277  ins_accel.z = last_received_packet.Data[9].Float;
278  ins_rates.p = last_received_packet.Data[10].Float;
279  ins_rates.q = last_received_packet.Data[11].Float;
280  ins_rates.r = last_received_packet.Data[12].Float;
281  break;
282  case VN100_REG_YMR :
283  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
284  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
285  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
286  ins_mag.x = last_received_packet.Data[3].Float;
287  ins_mag.y = last_received_packet.Data[4].Float;
288  ins_mag.z = last_received_packet.Data[5].Float;
289  ins_accel.x = last_received_packet.Data[6].Float;
290  ins_accel.y = last_received_packet.Data[7].Float;
291  ins_accel.z = last_received_packet.Data[8].Float;
292  ins_rates.p = last_received_packet.Data[9].Float;
293  ins_rates.q = last_received_packet.Data[10].Float;
294  ins_rates.r = last_received_packet.Data[11].Float;
295  break;
296  default:
297  break;
298  }
299 
300 }
301 
302 #ifndef DOWNLINK_DEVICE
303 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
304 #endif
305 #include "mcu_periph/uart.h"
306 #include "messages.h"
308 
309 extern void vn100_report_task( void ) {
310  DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
315  &ins_mag.x, &ins_mag.y, &ins_mag.z);
316 }
317 
#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:181
#define VN100_ADOR
Definition: ins_vn100.h:67
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:995
#define VN100_REG_YMR_SIZE
Definition: VN100.h:90
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
rotation matrix
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:149
#define VN100_REG_SBAUD_SIZE
Definition: VN100.h:68
uint8_t RegID
Definition: VN100.h:190
#define VN100_REG_QMR
Definition: VN100.h:48
uint8_t input_length
number of data words to read
Definition: spi.h:145
#define VN100_REG_YMR
Definition: VN100.h:60
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:150
angular rates
CPHA = 1.
Definition: spi.h:69
uint32_t UInt
Definition: VN100.h:170
float psi
in radians
#define VN100_SPI_DEV
Definition: ins_vn100.c:44
float Float
Definition: VN100.h:171
#define VN100_Error_None
Definition: VN100.h:104
uint8_t ErrID
Definition: VN100.h:191
#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
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
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
CPOL = 1.
Definition: spi.h:78
VN100_Param Data[VN100_SPI_BUFFER_SIZE]
Definition: VN100.h:192
float theta
in radians
struct FloatVect3 ins_mag
Definition: ins_vn100.c:62
euler angles
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
#define FALSE
Definition: imu_chimu.h:141
enum SPITransactionStatus status
Definition: spi.h:156
uint8_t CmdID
Definition: VN100.h:179
Architecture independent SPI (Serial Peripheral Interface) API.
void vn100_report_task(void)
Definition: ins_vn100.c:309
uint32_t ins_baud
Definition: ins_vn100.c:74
float p
in rad/s^2
#define INS_VN100_SET_ADOF
Definition: ins_vn100.h:80
Roation quaternion.
void vn100_event_task(void)
Definition: ins_vn100.c:157
Definition: spi.h:84
#define VN100_SLAVE_IDX
Definition: ins_vn100.c:49
struct FloatRMat ins_rmat
Definition: ins_vn100.c:60
float phi
in radians
uint32_t ins_ador
Definition: ins_vn100.c:72
bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit a spi transaction.
Definition: spi_arch.c:469
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
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 output_length
number of data words to write
Definition: spi.h:146
uint8_t RegID
Definition: VN100.h:180
void vn100_periodic_task(void)
Definition: ins_vn100.c:138
#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
float r
in rad/s^2
#define INS_VN100_SET_ADOR
Definition: ins_vn100.h:79
#define TRUE
Definition: imu_chimu.h:144
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
#define FLOAT_EULERS_OF_QUAT(_e, _q)
slave is selected before transaction and unselected after
Definition: spi.h:57
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:147
#define VN100_CmdID_WriteRegister
Definition: VN100.h:97
#define INS_VN100_READY
Definition: ins_vn100.h:81
float q
in rad/s^2
struct FloatEulers ins_eulers
Definition: ins_vn100.c:57
#define VN100_ADOF
Definition: ins_vn100.h:70
SPI transaction structure.
Definition: spi.h:142
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
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define VN100_CmdID_ReadRegister
Definition: VN100.h:96
struct FloatQuat ins_quat
Definition: ins_vn100.c:58
static bool_t ins_configure(void)
Definition: ins_vn100.c:108
struct FloatVect3 ins_accel
Definition: ins_vn100.c:61