Paparazzi UAS  v7.0_unstable
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"
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 
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 
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 
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 :
118  ins_init_status++;
119  break;
120  case INS_VN100_SET_ADOR :
123  ins_init_status++;
124  break;
125  case INS_VN100_SET_ADOF :
128  ins_init_status++;
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 ?
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 {
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 :
196  break;
197  case VN100_REG_ADOF :
199  break;
200  case VN100_REG_SBAUD :
202  break;
203  case VN100_REG_YPR :
207  break;
208  case VN100_REG_QTN :
214  break;
215  case VN100_REG_QTM :
224  break;
225  case VN100_REG_QTA :
234  break;
235  case VN100_REG_QTR :
244  break;
245  case VN100_REG_QMA :
257  break;
258  case VN100_REG_QAR :
270  break;
271  case VN100_REG_QMR :
286  break;
287  case VN100_REG_YMR :
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_CmdID_ReadRegister
Definition: VN100.h:96
uint8_t RegID
Definition: VN100.h:190
#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
VN100_Param Data[VN100_SPI_BUFFER_SIZE]
Definition: VN100.h:192
#define VN100_REG_ADOR
Definition: VN100.h:39
#define VN100_Error_None
Definition: VN100.h:104
uint8_t ErrID
Definition: VN100.h:191
#define VN100_REG_SBAUD_SIZE
Definition: VN100.h:68
#define VN100_REG_ADOF_SIZE
Definition: VN100.h:70
float Float
Definition: VN100.h:171
uint8_t CmdID
Definition: VN100.h:179
#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
uint32_t UInt
Definition: VN100.h:170
#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
uint8_t RegID
Definition: VN100.h:180
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
rotation matrix
angular rates
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:155
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:156
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
uint16_t input_length
number of data words to read
Definition: spi.h:151
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:153
uint16_t output_length
number of data words to write
Definition: spi.h:152
enum SPITransactionStatus status
Definition: spi.h:162
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:533
@ SPICphaEdge2
CPHA = 1.
Definition: spi.h:75
@ SPITransFailed
Definition: spi.h:100
@ SPITransSuccess
Definition: spi.h:99
@ SPITransDone
Definition: spi.h:101
@ SPICpolIdleHigh
CPOL = 1.
Definition: spi.h:84
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition: spi.h:63
@ SPIDss8bit
Definition: spi.h:90
SPI transaction structure.
Definition: spi.h:148
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
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
float ins_roll_neutral
Definition: ins_vn100.c:53
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
float ins_pitch_neutral
Definition: ins_vn100.c:54
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
volatile uint8_t ins_msg_received
Definition: ins_vn100.c:64
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.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98