Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_chimu_uart.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 // Output
9 #include "state.h"
10 
11 // For centripedal corrections
12 #include "subsystems/gps.h"
13 #include "subsystems/ahrs.h"
14 
15 #include "generated/airframe.h"
16 
17 #if CHIMU_DOWNLINK_IMMEDIATE
18 #include "mcu_periph/uart.h"
19 #include "pprzlink/messages.h"
21 #endif
22 
23 #include "ins_module.h"
24 #include "imu_chimu.h"
25 
26 #include "led.h"
27 
29 
32 
34 
35 static bool ahrs_chimu_enable_output(bool enable)
36 {
37  ahrs_chimu.is_enabled = enable;
38  return ahrs_chimu.is_enabled;
39 }
40 
42 {
45 }
46 
47 void ahrs_chimu_init(void)
48 {
49  ahrs_chimu.is_enabled = true;
50  ahrs_chimu.is_aligned = false;
51 
52  uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
53  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
54  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
55  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
56  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
57 
58  new_ins_attitude = 0;
59 
62 
64 
65  // Request Software version
66  for (int i = 0; i < 7; i++) {
67  InsUartSend1(ping[i]);
68  }
69 
70  // Quat Filter
71  for (int i = 0; i < 7; i++) {
72  InsUartSend1(quaternions[i]);
73  }
74 
75  // 50Hz
76  CHIMU_Checksum(rate, 12);
77  InsSend(rate, 12);
78 }
79 
80 
81 void parse_ins_msg(void)
82 {
83  struct link_device *dev = InsLinkDevice;
84  while (dev->char_available(dev->periph)) {
85  uint8_t ch = dev->get_byte(dev->periph);
86 
87  if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
88  if (CHIMU_DATA.m_MsgID == 0x03) {
89  new_ins_attitude = 1;
90  RunOnceEvery(25, LED_TOGGLE(3));
91  if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
92  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
93  }
94 
95  ahrs_chimu.is_aligned = true;
96 
97  if (ahrs_chimu.is_enabled) {
98  struct FloatEulers att = {
102  };
104  }
105 
106 #if CHIMU_DOWNLINK_IMMEDIATE
107  static uint8_t ahrs_chimu_id = AHRS_COMP_ID_CHIMU;
108  DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice,
112  &ahrs_chimu_id);
113 #endif
114 
115  }
116  }
117  }
118 }
MSG00_PING
#define MSG00_PING
Definition: imu_chimu.h:51
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
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
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
parse_ins_msg
void parse_ins_msg(void)
Definition: ahrs_chimu_uart.c:81
ins_pitch_neutral
INS_FORMAT ins_pitch_neutral
Definition: ahrs_chimu_uart.c:31
CHIMU_Euler::theta
float theta
Definition: imu_chimu.h:119
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
ahrs_chimu_enable_output
static bool ahrs_chimu_enable_output(bool enable)
Definition: ahrs_chimu_uart.c:35
InsSend
#define InsSend(_dat, _len)
Definition: ins_module.h:84
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
gps.h
Device independent GPS code (interface)
AHRS_COMP_ID_CHIMU
#define AHRS_COMP_ID_CHIMU
Definition: ahrs.h:42
ins_roll_neutral
INS_FORMAT ins_roll_neutral
Definition: ahrs_chimu_uart.c:30
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
CHIMU_DATA
CHIMU_PARSER_DATA CHIMU_DATA
Definition: ahrs_chimu_uart.c:28
CHIMU_PARSER_DATA::m_attitude
CHIMU_attitude_data m_attitude
Definition: imu_chimu.h:168
uint8_t
unsigned char uint8_t
Definition: types.h:14
led.h
arch independent LED (Light Emitting Diodes) API
CHIMU_attitude_data::euler
CHIMU_Euler euler
Definition: imu_chimu.h:135
InsUartSend1
#define InsUartSend1(c)
Definition: ins_module.h:83
ahrs.h
CHIMU_BROADCAST
#define CHIMU_BROADCAST
Definition: imu_chimu.h:48
ahrs_chimu_init
void ahrs_chimu_init(void)
Definition: ahrs_chimu_uart.c:47
AhrsChimu::is_aligned
bool is_aligned
Definition: ahrs_chimu.h:34
ahrs_chimu_register
void ahrs_chimu_register(void)
Definition: ahrs_chimu_uart.c:41
MSG10_UARTSETTINGS
#define MSG10_UARTSETTINGS
Definition: imu_chimu.h:67
AhrsChimu
Definition: ahrs_chimu.h:32
ahrs_chimu
struct AhrsChimu ahrs_chimu
Definition: ahrs_chimu_uart.c:33
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
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
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
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
MSG09_ESTIMATOR
#define MSG09_ESTIMATOR
Definition: imu_chimu.h:60