Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 #include "mcu_periph/uart.h"
23 #include "pprzlink/messages.h"
25 #endif
26 
27 #include "ins_module.h"
28 #include "imu_chimu.h"
29 #include "ahrs_chimu.h"
30 
31 #include "led.h"
32 
34 
37 
39 
40 void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d);
41 
42 #include "subsystems/abi.h"
46 #ifndef AHRS_CHIMU_GPS_ID
47 #define AHRS_CHIMU_GPS_ID GPS_MULTI_ID
48 #endif
49 PRINT_CONFIG_VAR(AHRS_CHIMU_GPS_ID)
51 static void gps_cb(uint8_t sender_id __attribute__((unused)),
52  uint32_t stamp __attribute__((unused)),
53  struct GpsState *gps_s)
54 {
55  ahrs_chimu_update_gps(gps_s->fix, gps_s->speed_3d);
56 }
57 
58 static bool ahrs_chimu_enable_output(bool enable)
59 {
60  ahrs_chimu.is_enabled = enable;
61  return ahrs_chimu.is_enabled;
62 }
63 
65 {
68  AbiBindMsgGPS(AHRS_CHIMU_GPS_ID, &gps_ev, gps_cb);
69 }
70 
71 void ahrs_chimu_init(void)
72 {
73  ahrs_chimu.is_enabled = true;
74  ahrs_chimu.is_aligned = false;
75 
76  // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
77  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
78  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
79  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
80  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
81 
82  new_ins_attitude = 0;
83 
86 
87  // Init
89 
90  // Quat Filter
91  CHIMU_Checksum(quaternions, 7);
92  InsSend(quaternions, 7);
93 
94  // Wait a bit (SPI send zero)
95  InsSend1(0);
96  InsSend1(0);
97  InsSend1(0);
98  InsSend1(0);
99  InsSend1(0);
100 
101  // 50Hz data: attitude only
102  CHIMU_Checksum(rate, 12);
103  InsSend(rate, 12);
104 }
105 
106 
107 void parse_ins_msg(void)
108 {
109  struct link_device *dev = InsLinkDevice;
110  while (dev->char_available(dev->periph)) {
111  uint8_t ch = dev->get_byte(dev->periph);
112 
113  if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
114  RunOnceEvery(25, LED_TOGGLE(3));
116  // TODO: remove this flag as it doesn't work with multiple AHRS
117  new_ins_attitude = 1;
118  if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
119  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
120  }
121 
122  //FIXME
123  ahrs_chimu.is_aligned = true;
124 
125  if (ahrs_chimu.is_enabled) {
126  struct FloatEulers att = {
130  };
132  struct FloatRates rates = {
135  0.
136  }; // FIXME rate r
137  stateSetBodyRates_f(&rates);
138  }
139  } else if (CHIMU_DATA.m_MsgID == 0x02) {
140 #if CHIMU_DOWNLINK_IMMEDIATE
141  RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
143 #endif
144  }
145  }
146  }
147 }
148 
149 
150 void ahrs_chimu_update_gps(uint8_t gps_fix __attribute__((unused)), uint16_t gps_speed_3d)
151 {
152  // Send SW Centripetal Corrections
153  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
154 
155  float gps_speed = 0;
156 
157  if (GpsFixValid()) {
158  gps_speed = gps_speed_3d / 100.;
159  }
160  gps_speed = FloatSwap(gps_speed);
161 
162  memmove(&centripedal[6], &gps_speed, 4);
163 
164  // Fill X-speed
165 
166  CHIMU_Checksum(centripedal, 19);
167  InsSend(centripedal, 19);
168 
169  // Downlink Send
170 }
uint16_t
unsigned short uint16_t
Definition: types.h:16
CHIMU_STX
#define CHIMU_STX
Definition: imu_chimu.h:47
INS_FORMAT
#define INS_FORMAT
Definition: ins_module.h:35
CHIMU_Init
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:108
ahrs_chimu_init
void ahrs_chimu_init(void)
Definition: ahrs_chimu_spi.c:71
AhrsChimu::is_enabled
bool is_enabled
Definition: ahrs_chimu.h:33
CHIMU_PARSER_DATA
Definition: imu_chimu.h:156
INS_PITCH_NEUTRAL_DEFAULT
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
abi.h
CHIMU_DATA
CHIMU_PARSER_DATA CHIMU_DATA
Definition: ahrs_chimu_spi.c:33
ahrs_chimu.h
spi.h
imu_chimu.h
InsLinkDevice
#define InsLinkDevice
Definition: ins_module.h:76
INS_ROLL_NEUTRAL_DEFAULT
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
GpsState
data structure for GPS information
Definition: gps.h:87
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
CHIMU_Euler::theta
float theta
Definition: imu_chimu.h:119
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
InsSend
#define InsSend(_dat, _len)
Definition: ins_module.h:84
InsSend1
#define InsSend1(c)
Definition: ins_module.h:82
ins_roll_neutral
INS_FORMAT ins_roll_neutral
Definition: ahrs_chimu_spi.c:35
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
gps.h
Device independent GPS code (interface)
CHIMU_PARSER_DATA::m_attrates
CHIMU_attitude_data m_attrates
Definition: imu_chimu.h:169
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
CHIMU_PARSER_DATA::m_attitude
CHIMU_attitude_data m_attitude
Definition: imu_chimu.h:168
AHRS_CHIMU_GPS_ID
#define AHRS_CHIMU_GPS_ID
ABI binding for gps data.
Definition: ahrs_chimu_spi.c:47
uint8_t
unsigned char uint8_t
Definition: types.h:14
ins_pitch_neutral
INS_FORMAT ins_pitch_neutral
Definition: ahrs_chimu_spi.c:36
ahrs_chimu_enable_output
static bool ahrs_chimu_enable_output(bool enable)
Definition: ahrs_chimu_spi.c:58
led.h
arch independent LED (Light Emitting Diodes) API
CHIMU_sensor_data::rate
float rate[3]
Definition: imu_chimu.h:149
CHIMU_attitude_data::euler
CHIMU_Euler euler
Definition: imu_chimu.h:135
ahrs.h
CHIMU_BROADCAST
#define CHIMU_BROADCAST
Definition: imu_chimu.h:48
GpsFixValid
#define GpsFixValid()
Definition: gps.h:43
AhrsChimu::is_aligned
bool is_aligned
Definition: ahrs_chimu.h:34
MSG10_UARTSETTINGS
#define MSG10_UARTSETTINGS
Definition: imu_chimu.h:67
AhrsChimu
Definition: ahrs_chimu.h:32
ahrs_chimu_register
void ahrs_chimu_register(void)
Definition: ahrs_chimu_spi.c:64
ahrs_chimu
struct AhrsChimu ahrs_chimu
Definition: ahrs_chimu_spi.c:38
spi_slave_hs_arch.h
parse_ins_msg
void parse_ins_msg(void)
Definition: ahrs_chimu_spi.c:107
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
ahrs_chimu_update_gps
void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d)
Definition: ahrs_chimu_spi.c:150
CHIMU_Euler::phi
float phi
Definition: imu_chimu.h:118
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
ahrs_register_impl
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
state.h
FloatSwap
#define FloatSwap(X)
Definition: imu_chimu.h:112
CHIMU_Parse
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:149
CHIMU_Euler::psi
float psi
Definition: imu_chimu.h:120
ins_module.h
Device independent INS code.
new_ins_attitude
volatile uint8_t new_ins_attitude
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ahrs_chimu_spi.c:51
CHIMU_Msg_3_IMU_Attitude
#define CHIMU_Msg_3_IMU_Attitude
Definition: imu_chimu.h:74
CHIMU_PARSER_DATA::m_MsgID
unsigned char m_MsgID
Definition: imu_chimu.h:162
CHIMU_Checksum
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
Definition: imu_chimu.c:82
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
MSG09_ESTIMATOR
#define MSG09_ESTIMATOR
Definition: imu_chimu.h:60
gps_ev
static abi_event gps_ev
Definition: ahrs_chimu_spi.c:50
CHIMU_PARSER_DATA::m_sensor
CHIMU_sensor_data m_sensor
Definition: imu_chimu.h:170