Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ins_vn100.c
Go to the documentation of this file.
1 /*
2  * Paparazzi $Id: ins_xsens.c 3872 2009-08-05 14:42:41Z mmm $
3  *
4  * Copyright (C) 2010 ENAC
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
29 #include "ins_vn100.h"
30 
31 #include "generated/airframe.h"
32 #include "led.h"
33 
35 #include "messages.h"
36 
37 /* neutrals */
41 
48 
50 
51 /* last received SPI packet */
53 /* last send packet */
55 
56 /* output mode */
60 
62 
63 /* ins_init and ins_periodic_task to be implemented according to the airframe type : FW or BOOZ */
64 
65 void parse_ins_msg( void ) {
66  if (last_received_packet.ErrID != VN100_Error_None) {
67  //TODO send error
68  return;
69  }
70 
71  // parse message (will work only with read and write register)
72  switch (last_received_packet.RegID) {
73  case VN100_REG_ADOR :
74  ins_ador = last_received_packet.Data[0].UInt;
75  break;
76  case VN100_REG_ADOF :
77  ins_adof = last_received_packet.Data[0].UInt;
78  break;
79  case VN100_REG_SBAUD :
80  ins_baud = last_received_packet.Data[0].UInt;
81  break;
82  case VN100_REG_YPR :
83  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
84  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
85  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
86  break;
87  case VN100_REG_QTN :
88  ins_quat.qi = last_received_packet.Data[0].Float;
89  ins_quat.qx = last_received_packet.Data[1].Float;
90  ins_quat.qy = last_received_packet.Data[2].Float;
91  ins_quat.qz = last_received_packet.Data[3].Float;
93  break;
94  case VN100_REG_QTM :
95  ins_quat.qi = last_received_packet.Data[0].Float;
96  ins_quat.qx = last_received_packet.Data[1].Float;
97  ins_quat.qy = last_received_packet.Data[2].Float;
98  ins_quat.qz = last_received_packet.Data[3].Float;
100  ins_mag.x = last_received_packet.Data[4].Float;
101  ins_mag.y = last_received_packet.Data[5].Float;
102  ins_mag.z = last_received_packet.Data[6].Float;
103  break;
104  case VN100_REG_QTA :
105  ins_quat.qi = last_received_packet.Data[0].Float;
106  ins_quat.qx = last_received_packet.Data[1].Float;
107  ins_quat.qy = last_received_packet.Data[2].Float;
108  ins_quat.qz = last_received_packet.Data[3].Float;
110  ins_accel.x = last_received_packet.Data[4].Float;
111  ins_accel.y = last_received_packet.Data[5].Float;
112  ins_accel.z = last_received_packet.Data[6].Float;
113  break;
114  case VN100_REG_QTR :
115  ins_quat.qi = last_received_packet.Data[0].Float;
116  ins_quat.qx = last_received_packet.Data[1].Float;
117  ins_quat.qy = last_received_packet.Data[2].Float;
118  ins_quat.qz = last_received_packet.Data[3].Float;
120  ins_rates.p = last_received_packet.Data[4].Float;
121  ins_rates.q = last_received_packet.Data[5].Float;
122  ins_rates.r = last_received_packet.Data[6].Float;
123  break;
124  case VN100_REG_QMA :
125  ins_quat.qi = last_received_packet.Data[0].Float;
126  ins_quat.qx = last_received_packet.Data[1].Float;
127  ins_quat.qy = last_received_packet.Data[2].Float;
128  ins_quat.qz = last_received_packet.Data[3].Float;
130  ins_mag.x = last_received_packet.Data[4].Float;
131  ins_mag.y = last_received_packet.Data[5].Float;
132  ins_mag.z = last_received_packet.Data[6].Float;
133  ins_accel.x = last_received_packet.Data[7].Float;
134  ins_accel.y = last_received_packet.Data[8].Float;
135  ins_accel.z = last_received_packet.Data[9].Float;
136  break;
137  case VN100_REG_QAR :
138  ins_quat.qi = last_received_packet.Data[0].Float;
139  ins_quat.qx = last_received_packet.Data[1].Float;
140  ins_quat.qy = last_received_packet.Data[2].Float;
141  ins_quat.qz = last_received_packet.Data[3].Float;
143  ins_accel.x = last_received_packet.Data[4].Float;
144  ins_accel.y = last_received_packet.Data[5].Float;
145  ins_accel.z = last_received_packet.Data[6].Float;
146  ins_rates.p = last_received_packet.Data[7].Float;
147  ins_rates.q = last_received_packet.Data[8].Float;
148  ins_rates.r = last_received_packet.Data[9].Float;
149  break;
150  case VN100_REG_QMR :
151  ins_quat.qi = last_received_packet.Data[0].Float;
152  ins_quat.qx = last_received_packet.Data[1].Float;
153  ins_quat.qy = last_received_packet.Data[2].Float;
154  ins_quat.qz = last_received_packet.Data[3].Float;
156  ins_mag.x = last_received_packet.Data[4].Float;
157  ins_mag.y = last_received_packet.Data[5].Float;
158  ins_mag.z = last_received_packet.Data[6].Float;
159  ins_accel.x = last_received_packet.Data[7].Float;
160  ins_accel.y = last_received_packet.Data[8].Float;
161  ins_accel.z = last_received_packet.Data[9].Float;
162  ins_rates.p = last_received_packet.Data[10].Float;
163  ins_rates.q = last_received_packet.Data[11].Float;
164  ins_rates.r = last_received_packet.Data[12].Float;
165  break;
166  case VN100_REG_YMR :
167  ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
168  ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
169  ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
170  ins_mag.x = last_received_packet.Data[3].Float;
171  ins_mag.y = last_received_packet.Data[4].Float;
172  ins_mag.z = last_received_packet.Data[5].Float;
173  ins_accel.x = last_received_packet.Data[6].Float;
174  ins_accel.y = last_received_packet.Data[7].Float;
175  ins_accel.z = last_received_packet.Data[8].Float;
176  ins_rates.p = last_received_packet.Data[9].Float;
177  ins_rates.q = last_received_packet.Data[10].Float;
178  ins_rates.r = last_received_packet.Data[11].Float;
179  break;
180  }
181 
182 }
183 
184 #ifndef DOWNLINK_DEVICE
185 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
186 #endif
187 #include "mcu_periph/uart.h"
188 #include "messages.h"
190 
191 extern void ins_report_task( void ) {
192  DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
197  &ins_mag.x, &ins_mag.y, &ins_mag.z);
198 }
#define VN100_REG_QTR
Definition: VN100.h:47
float ins_roll_neutral
driver for the VectorNav VN100
Definition: ins_vn100.c:38
#define VN100_REG_QAR
Definition: VN100.h:49
struct FloatRates ins_rates
Definition: ins_vn100.c:44
#define VN100_REG_SBAUD
Definition: VN100.h:40
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
rotation matrix
uint8_t RegID
Definition: VN100.h:192
#define VN100_REG_QMR
Definition: VN100.h:50
#define VN100_REG_YMR
Definition: VN100.h:62
angular rates
void ins_report_task(void)
Definition: ins_vn100.c:191
uint32_t UInt
Definition: VN100.h:172
float psi
in radians
float Float
Definition: VN100.h:173
#define VN100_Error_None
Definition: VN100.h:106
uint8_t ErrID
Definition: VN100.h:193
#define VN100_REG_QTM
Definition: VN100.h:45
#define VN100_REG_ADOR
Definition: VN100.h:41
#define VN100_REG_ADOF
Definition: VN100.h:42
VN100_Req_Packet last_send_packet
Definition: ins_vn100.c:54
VN100_Param Data[VN100_SPI_BUFFER_SIZE]
Definition: VN100.h:194
float theta
in radians
struct FloatVect3 ins_mag
Definition: ins_vn100.c:47
euler angles
uint8_t ins_init_status
Definition: ins_vn100.c:61
volatile uint8_t ins_msg_received
Definition: ins_vn100.c:49
float ins_yaw_neutral
Definition: ins_vn100.c:40
uint32_t ins_baud
Definition: ins_vn100.c:59
float p
in rad/s^2
Roation quaternion.
struct FloatRMat ins_rmat
Definition: ins_vn100.c:45
float phi
in radians
uint32_t ins_ador
Definition: ins_vn100.c:57
unsigned long uint32_t
Definition: types.h:18
#define VN100_REG_QTA
Definition: VN100.h:46
uint32_t ins_adof
Definition: ins_vn100.c:58
#define VN100_REG_QTN
Definition: VN100.h:44
float r
in rad/s^2
float ins_pitch_neutral
Definition: ins_vn100.c:39
void parse_ins_msg(void)
Definition: ins_vn100.c:65
unsigned char uint8_t
Definition: types.h:14
#define VN100_REG_QMA
Definition: VN100.h:48
#define FLOAT_EULERS_OF_QUAT(_e, _q)
arch independent LED (Light Emitting Diodes) API
float q
in rad/s^2
struct FloatEulers ins_eulers
Definition: ins_vn100.c:42
Interface for the VectorNav VN100 AHRS use the binary protocal on the SPI link.
#define VN100_REG_YPR
Definition: VN100.h:43
VN100_Res_Packet last_received_packet
Definition: ins_vn100.c:52
struct FloatQuat ins_quat
Definition: ins_vn100.c:43
struct FloatVect3 ins_accel
Definition: ins_vn100.c:46