Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
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
79static inline void parse_ins_msg(void);
80
81/* spi transaction */
83
84/* init vn100 */
85void vn100_init(void)
86{
87
88 //ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
89 //ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
91
92 vn100_trans.slave_idx = VN100_SLAVE_IDX;
97 vn100_trans.output_buf = (uint8_t *)&last_send_packet;
100
104
106
107}
108
109static 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 :
117 vn100_trans.output_length = 4 + VN100_REG_SBAUD_SIZE;
119 break;
120 case INS_VN100_SET_ADOR :
122 vn100_trans.output_length = 4 + VN100_REG_ADOR_SIZE;
124 break;
125 case INS_VN100_SET_ADOF :
127 vn100_trans.output_length = 4 + VN100_REG_ADOF_SIZE;
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 ?
153 vn100_trans.input_length = 4 + VN100_REG_YMR_SIZE;
154 // submit
156 }
157
158}
159
161{
162 if (vn100_trans.status == SPITransSuccess) {
164#ifndef INS_VN100_READ_ONLY
165 // Update estimator
166 // FIXME Use a proper rotation matrix here
167 struct FloatEulers att = {
170 ins_eulers.psi
171 };
172 stateSetNedToBodyEulers_f(MODULE_INS_VN100_ID, &att);
173 stateSetBodyRates_f(MODULE_INS_VN100_ID, &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
185static 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 :
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
311extern 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
#define VN100_CmdID_ReadRegister
Definition VN100.h:96
#define VN100_REG_QMR
Definition VN100.h:48
#define VN100_REG_QTM
Definition VN100.h:43
#define VN100_REG_ADOR_SIZE
Definition VN100.h:69
#define VN100_REG_QAR
Definition VN100.h:47
#define VN100_REG_ADOR
Definition VN100.h:39
#define VN100_Error_None
Definition VN100.h:104
#define VN100_REG_SBAUD_SIZE
Definition VN100.h:68
#define VN100_REG_ADOF_SIZE
Definition VN100.h:70
#define VN100_CmdID_WriteRegister
Definition VN100.h:97
#define VN100_REG_YPR
Definition VN100.h:41
#define VN100_REG_QTA
Definition VN100.h:44
#define VN100_REG_QTN
Definition VN100.h:42
#define VN100_REG_QTR
Definition VN100.h:45
#define VN100_REG_SBAUD
Definition VN100.h:38
#define VN100_REG_YMR
Definition VN100.h:60
#define VN100_REG_YMR_SIZE
Definition VN100.h:90
#define VN100_REG_ADOF
Definition VN100.h:40
#define VN100_REG_QMA
Definition VN100.h:46
float ins_roll_neutral
Definition ins_arduimu.c:14
float ins_pitch_neutral
Definition ins_arduimu.c:15
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
rotation matrix
angular rates
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition spi_arch.c:548
@ SPICphaEdge2
CPHA = 1.
Definition spi.h:71
@ SPITransFailed
Definition spi.h:96
@ SPITransSuccess
Definition spi.h:95
@ SPITransDone
Definition spi.h:97
@ SPICpolIdleHigh
CPOL = 1.
Definition spi.h:80
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition spi.h:59
@ SPIDss8bit
Definition spi.h:86
SPI transaction structure.
Definition spi.h:144
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition state.h:1286
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
volatile uint8_t ins_msg_received
Definition ins_vn100.c:64
float ins_yaw_neutral
Definition ins_vn100.c:55
#define VN100_SPI_DEV
Definition ins_vn100.c:44
VN100_Req_Packet last_send_packet
Definition ins_vn100.c:69
struct FloatRMat ins_rmat
Definition ins_vn100.c:60
VN100_Res_Packet last_received_packet
Definition ins_vn100.c:67
uint32_t ins_adof
Definition ins_vn100.c:73
static bool ins_configure(void)
Definition ins_vn100.c:109
void vn100_periodic_task(void)
Definition ins_vn100.c:140
#define INS_YAW_NEUTRAL_DEFAULT
driver for the VectorNav VN100
Definition ins_vn100.c:39
void vn100_report_task(void)
Definition ins_vn100.c:311
void vn100_event_task(void)
Definition ins_vn100.c:160
void vn100_init(void)
Definition ins_vn100.c:85
uint8_t ins_init_status
Definition ins_vn100.c:76
struct FloatVect3 ins_mag
Definition ins_vn100.c:62
static void parse_ins_msg(void)
Definition ins_vn100.c:185
uint32_t ins_baud
Definition ins_vn100.c:74
struct FloatVect3 ins_accel
Definition ins_vn100.c:61
struct FloatEulers ins_eulers
Definition ins_vn100.c:57
#define VN100_SLAVE_IDX
Definition ins_vn100.c:49
struct FloatRates ins_rates
Definition ins_vn100.c:59
struct FloatQuat ins_quat
Definition ins_vn100.c:58
struct spi_transaction vn100_trans
Definition ins_vn100.c:82
uint32_t ins_ador
Definition ins_vn100.c:72
Interface for the VectorNav VN100 AHRS use the binary protocal on the SPI link.
#define VN100_BAUD
Definition ins_vn100.h:73
#define INS_VN100_SET_BAUD
Definition ins_vn100.h:78
#define INS_VN100_READY
Definition ins_vn100.h:81
#define VN100_ADOF
Definition ins_vn100.h:70
#define VN100_ADOR
Definition ins_vn100.h:67
#define INS_VN100_SET_ADOR
Definition ins_vn100.h:79
#define INS_VN100_SET_ADOF
Definition ins_vn100.h:80
Architecture independent SPI (Serial Peripheral Interface) API.
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.