Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ins_chimu_spi.c
Go to the documentation of this file.
1 /*
2  C code to connect a CHIMU using uart
3 */
4 
5 
6 #include <stdbool.h>
7 
8 // SPI
9 #include "mcu_periph/spi.h"
11 
12 // Output
13 #include "estimator.h"
14 
15 // For centripedal corrections
16 #include "subsystems/gps.h"
17 
18 // Telemetry
19 #ifndef DOWNLINK_DEVICE
20 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
21 #endif
22 
23 #include "mcu_periph/uart.h"
24 #include "messages.h"
26 
27 #include "ins_chimu_spi.h"
28 #include "imu_chimu.h"
29 
30 
32 
35 
37 
38 void ins_init( void )
39 {
40  // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
41  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
42  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
43  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
44  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
45 
46  new_ins_attitude = 0;
47 
48  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
49  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
50 
51  // Init
52  CHIMU_Init(&CHIMU_DATA);
53 
54  // Quat Filter
55  CHIMU_Checksum(quaternions,7);
56  InsSend(quaternions,7);
57 
58  // Wait a bit (SPI send zero)
59  InsSend1(0);
60  InsSend1(0);
61  InsSend1(0);
62  InsSend1(0);
63  InsSend1(0);
64 
65  // 50Hz data: attitude only
66  CHIMU_Checksum(rate,12);
67  InsSend(rate,12);
68 }
69 
70 void parse_ins_msg( void )
71 {
72  while (InsLink(ChAvailable())) {
73  uint8_t ch = InsLink(Getch());
74 
75  if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
76  RunOnceEvery(25, LED_TOGGLE(3) );
77  if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) {
78  new_ins_attitude = 1;
79  if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
80  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
81  }
82 
83  EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
84  EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r
85  }
86  else if(CHIMU_DATA.m_MsgID==0x02) {
87 
88  RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
89 
90  }
91  }
92  }
93 }
94 
95 
96 void chimu_update_gps( void )
97 {
98  // Send SW Centripetal Corrections
99  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
100 
101  float gps_speed = 0;
102 
103  if (gps.fix == GPS_FIX_3D) {
104  gps_speed = gps.speed_3d/100.;
105  }
106  gps_speed = FloatSwap(gps_speed);
107 
108  memmove (&centripedal[6], &gps_speed, 4);
109 
110  // Fill X-speed
111 
112  CHIMU_Checksum(centripedal,19);
113  InsSend(centripedal,19);
114 
115  // Downlink Send
116 }
117 
118 //Frequency defined in conf *.xml
119 void ins_periodic_task( void )
120 {
121 }
122 
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define FloatSwap(X)
Definition: imu_chimu.h:113
void ins_periodic_task(void)
#define CHIMU_Msg_3_IMU_Attitude
Definition: imu_chimu.h:74
CHIMU_Euler euler
Definition: imu_chimu.h:136
#define EstimatorSetAtt(phi, psi, theta)
Definition: estimator.h:135
INS_FORMAT ins_roll_neutral
Definition: ins_chimu_spi.c:33
CHIMU_sensor_data m_sensor
Definition: imu_chimu.h:171
#define MSG10_UARTSETTINGS
Definition: imu_chimu.h:67
#define CHIMU_STX
Definition: imu_chimu.h:47
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
Definition: imu_chimu.c:83
void chimu_update_gps(void)
Definition: ins_chimu_spi.c:96
arch independent SPI (Serial Peripheral Interface) API
CHIMU_attitude_data m_attrates
Definition: imu_chimu.h:170
volatile uint8_t new_ins_attitude
Definition: ins_chimu_spi.c:36
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:109
Device independent GPS code (interface)
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:150
#define InsSend(_dat, _len)
Definition: ins_module.h:89
CHIMU_attitude_data m_attitude
Definition: imu_chimu.h:169
float phi
Definition: imu_chimu.h:119
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define InsLink(_x)
Definition: ins_module.h:83
#define INS_FORMAT
Definition: ins_module.h:38
void ins_init(void)
Definition: ins_chimu_spi.c:38
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
INS_FORMAT ins_pitch_neutral
Definition: ins_chimu_spi.c:34
CHIMU_PARSER_DATA CHIMU_DATA
Definition: ins_chimu_spi.c:31
unsigned char uint8_t
Definition: types.h:14
float psi
Definition: imu_chimu.h:121
#define CHIMU_BROADCAST
Definition: imu_chimu.h:48
State estimation, fusioning sensors.
float theta
Definition: imu_chimu.h:120
#define MSG09_ESTIMATOR
Definition: imu_chimu.h:60
#define InsSend1(c)
Definition: ins_module.h:87
struct GpsState gps
global GPS state
Definition: gps.c:31
unsigned char m_MsgID
Definition: imu_chimu.h:163
#define EstimatorSetRate(p, q, r)
Definition: estimator.h:138
void parse_ins_msg(void)
Definition: ins_chimu_spi.c:70