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
fw_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 "modules/ins/ins_vn100.h"
30 #include "mcu_periph/spi.h"
31 #include "estimator.h"
32 #include "generated/airframe.h"
33 
34 #ifndef INS_YAW_NEUTRAL_DEFAULT
35 #define INS_YAW_NEUTRAL_DEFAULT 0.
36 #endif
37 
38 void ins_init( void ) {
39 
40  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
41  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
43 
44  /* SPI polarity = 1 - data sampled on rising edge */
45  SpiSetCPOL();
46  /* SPI phase = 1 - SCK idle high */
47  SpiSetCPHA();
48 
52 
54 
55 }
56 
57 static inline bool_t ins_configure( void ) {
58  switch (ins_init_status) {
59  case INS_VN100_SET_BAUD :
63  break;
64  case INS_VN100_SET_ADOR :
68  break;
69  case INS_VN100_SET_ADOF :
73  break;
74  case INS_VN100_READY :
75  return TRUE;
76  }
80  SpiSelectSlave0();
81  SpiStart();
82  return FALSE;
83 }
84 
85 void ins_periodic_task( void ) {
86  if (!SpiCheckAvailable()) {
87  SpiOverRun();
88  return;
89  }
90 
91  if (!ins_configure()) return;
92 
93  // Fill request for QMR
96 
100  SpiSelectSlave0();
101  SpiStart();
102 
103 }
104 
105 #include "mcu_periph/uart.h"
106 #include "messages.h"
108 
109 void ins_event_task( void ) {
110  if (spi_message_received) {
112  parse_ins_msg();
113 #ifndef INS_VN100_READ_ONLY
114  // Update estimator
115  // FIXME Use a proper rotation matrix here
117 #endif
118  //uint8_t s = 4+VN100_REG_QMR_SIZE;
119  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
120  }
121 }
122 
#define VN100_ADOR
Definition: ins_vn100.h:70
#define VN100_REG_SBAUD
Definition: VN100.h:40
#define VN100_REG_YMR_SIZE
Definition: VN100.h:92
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define SpiSetCPOL()
Definition: spi_arch.h:188
#define VN100_REG_SBAUD_SIZE
Definition: VN100.h:70
bool_t spi_message_received
Definition: sim_baro.c:8
#define VN100_REG_YMR
Definition: VN100.h:62
float psi
in radians
#define EstimatorSetAtt(phi, psi, theta)
Definition: estimator.h:131
#define VN100_REG_ADOR
Definition: VN100.h:41
#define VN100_REG_ADOF_SIZE
Definition: VN100.h:72
#define SpiSetCPHA()
Definition: spi_arch.h:191
#define VN100_REG_ADOF
Definition: VN100.h:42
VN100_Req_Packet last_send_packet
Definition: ins_vn100.c:54
void ins_event_task(void)
Definition: fw_ins_vn100.c:109
uint8_t * spi_buffer_output
Definition: spi.c:29
float theta
in radians
uint8_t ins_init_status
Definition: ins_vn100.c:61
uint8_t * spi_buffer_input
Definition: spi.c:28
float ins_yaw_neutral
Definition: ins_vn100.c:40
#define FALSE
Definition: imu_chimu.h:141
uint8_t CmdID
Definition: VN100.h:181
arch independent SPI (Serial Peripheral Interface) API
uint32_t ins_baud
Definition: ins_vn100.c:59
#define INS_VN100_SET_ADOF
Definition: ins_vn100.h:83
static bool_t ins_configure(void)
Definition: fw_ins_vn100.c:57
float phi
in radians
uint32_t ins_ador
Definition: ins_vn100.c:57
uint32_t ins_adof
Definition: ins_vn100.c:58
uint8_t RegID
Definition: VN100.h:182
#define INS_YAW_NEUTRAL_DEFAULT
driver for the VectorNav VN100 (Fixed-Wing part)
Definition: fw_ins_vn100.c:35
#define VN100_REG_ADOR_SIZE
Definition: VN100.h:71
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define INS_VN100_SET_ADOR
Definition: ins_vn100.h:82
void ins_periodic_task(void)
Definition: fw_ins_vn100.c:85
void ins_init(void)
Definition: fw_ins_vn100.c:38
#define TRUE
Definition: imu_chimu.h:144
#define INS_VN100_SET_BAUD
Definition: ins_vn100.h:81
#define VN100_BAUD
Definition: ins_vn100.h:76
unsigned char uint8_t
Definition: types.h:14
#define VN100_CmdID_WriteRegister
Definition: VN100.h:99
#define INS_VN100_READY
Definition: ins_vn100.h:84
State estimation, fusioning sensors.
struct FloatEulers ins_eulers
Definition: ins_vn100.c:42
float ins_roll_neutral
Definition: ins_arduimu.c:14
#define VN100_ADOF
Definition: ins_vn100.h:73
Interface for the VectorNav VN100 AHRS use the binary protocal on the SPI link.
VN100_Res_Packet last_received_packet
Definition: ins_vn100.c:52
#define VN100_CmdID_ReadRegister
Definition: VN100.h:98
uint8_t spi_buffer_length
Definition: spi.c:30
void parse_ins_msg(void)
Definition: ins_chimu_spi.c:70