Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_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 "state.h"
14 
15 // For centripedal corrections
16 #include "subsystems/gps.h"
17 #include "subsystems/ahrs.h"
18 
19 #include "generated/airframe.h"
20 
21 #if CHIMU_DOWNLINK_IMMEDIATE
22 #ifndef DOWNLINK_DEVICE
23 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
24 #endif
25 #include "mcu_periph/uart.h"
26 #include "messages.h"
28 #endif
29 
30 #include "ins_module.h"
31 #include "imu_chimu.h"
32 
33 #include "led.h"
34 
36 
39 
40 void ahrs_init(void)
41 {
43 
44  // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
45  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
46  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
47  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
48  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
49 
50  new_ins_attitude = 0;
51 
54 
55  // Init
56  CHIMU_Init(&CHIMU_DATA);
57 
58  // Quat Filter
59  CHIMU_Checksum(quaternions,7);
60  InsSend(quaternions,7);
61 
62  // Wait a bit (SPI send zero)
63  InsSend1(0);
64  InsSend1(0);
65  InsSend1(0);
66  InsSend1(0);
67  InsSend1(0);
68 
69  // 50Hz data: attitude only
70  CHIMU_Checksum(rate,12);
71  InsSend(rate,12);
72 }
73 
74 void ahrs_align(void)
75 {
77 }
78 
79 void parse_ins_msg( void )
80 {
81  while (InsLink(ChAvailable())) {
82  uint8_t ch = InsLink(Getch());
83 
84  if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
85  RunOnceEvery(25, LED_TOGGLE(3) );
86  if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) {
87  new_ins_attitude = 1;
88  if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
89  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
90  }
91 
92  struct FloatEulers att = {
93  CHIMU_DATA.m_attitude.euler.phi,
94  CHIMU_DATA.m_attitude.euler.theta,
95  CHIMU_DATA.m_attitude.euler.psi
96  };
98  struct FloatRates rates = {
99  CHIMU_DATA.m_sensor.rate[0],
100  CHIMU_DATA.m_attrates.euler.theta,
101  0.
102  }; // FIXME rate r
103  stateSetBodyRates_f(&rates);
104  }
105  else if(CHIMU_DATA.m_MsgID==0x02) {
106 #if CHIMU_DOWNLINK_IMMEDIATE
107  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]));
108 #endif
109  }
110  }
111  }
112 }
113 
114 
115 void ahrs_update_gps(void)
116 {
117  // Send SW Centripetal Corrections
118  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
119 
120  float gps_speed = 0;
121 
122  if (gps.fix == GPS_FIX_3D) {
123  gps_speed = gps.speed_3d/100.;
124  }
125  gps_speed = FloatSwap(gps_speed);
126 
127  memmove (&centripedal[6], &gps_speed, 4);
128 
129  // Fill X-speed
130 
131  CHIMU_Checksum(centripedal,19);
132  InsSend(centripedal,19);
133 
134  // Downlink Send
135 }
136 
137 void ahrs_propagate(void) {
138 }
139 
140 void ahrs_update_accel(void) {
141 }
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define FloatSwap(X)
Definition: imu_chimu.h:113
Attitude and Heading Reference System interface.
#define CHIMU_Msg_3_IMU_Attitude
Definition: imu_chimu.h:74
CHIMU_Euler euler
Definition: imu_chimu.h:136
angular rates
void ahrs_propagate(void)
Propagation.
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
CHIMU_sensor_data m_sensor
Definition: imu_chimu.h:171
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
#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:78
#define GPS_FIX_3D
Definition: gps.h:43
euler angles
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
Definition: imu_chimu.c:83
Architecture independent SPI (Serial Peripheral Interface) API.
INS_FORMAT ins_roll_neutral
CHIMU_attitude_data m_attrates
Definition: imu_chimu.h:170
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:109
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define INS_PITCH_NEUTRAL_DEFAULT
INS_FORMAT ins_pitch_neutral
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:85
void parse_ins_msg(void)
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:79
#define INS_FORMAT
Definition: ins_module.h:35
#define AHRS_UNINIT
Definition: ahrs.h:35
CHIMU_PARSER_DATA CHIMU_DATA
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
volatile uint8_t new_ins_attitude
float psi
Definition: imu_chimu.h:121
#define CHIMU_BROADCAST
Definition: imu_chimu.h:48
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
float theta
Definition: imu_chimu.h:120
arch independent LED (Light Emitting Diodes) API
#define MSG09_ESTIMATOR
Definition: imu_chimu.h:60
Highspeed SPI Slave Interface.
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
void ahrs_align(void)
Aligns the AHRS.
#define InsSend1(c)
Definition: ins_module.h:83
struct GpsState gps
global GPS state
Definition: gps.c:33
Device independent INS code.
unsigned char m_MsgID
Definition: imu_chimu.h:163
#define AHRS_RUNNING
Definition: ahrs.h:36
void ahrs_init(void)
AHRS initialization.