Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "messages.h"
21 #endif
22 
23 #include "ins_module.h"
24 #include "imu_chimu.h"
25 
26 #include "led.h"
27 
29 
32 
33 void ahrs_init(void)
34 {
36 
37  uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
38  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
39  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
40  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
41  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
42 
43  new_ins_attitude = 0;
44 
47 
48  CHIMU_Init(&CHIMU_DATA);
49 
50  // Request Software version
51  for (int i=0;i<7;i++) {
52  InsUartSend1(ping[i]);
53  }
54 
55  // Quat Filter
56  for (int i=0;i<7;i++) {
57  InsUartSend1(quaternions[i]);
58  }
59 
60  // 50Hz
61  CHIMU_Checksum(rate,12);
62  InsSend(rate,12);
63 }
64 void ahrs_align(void)
65 {
67 }
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  if(CHIMU_DATA.m_MsgID==0x03) {
77  new_ins_attitude = 1;
78  RunOnceEvery(25, LED_TOGGLE(3) );
79  if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
80  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
81  }
82 
83  struct FloatEulers att = {
84  CHIMU_DATA.m_attitude.euler.phi,
85  CHIMU_DATA.m_attitude.euler.theta,
86  CHIMU_DATA.m_attitude.euler.psi
87  };
89 #if CHIMU_DOWNLINK_IMMEDIATE
90  DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
91 #endif
92 
93  }
94  }
95  }
96 }
97 
98 void ahrs_update_gps( void )
99 {
100 }
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
Attitude and Heading Reference System interface.
CHIMU_Euler euler
Definition: imu_chimu.h:136
INS_FORMAT ins_roll_neutral
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
#define MSG10_UARTSETTINGS
Definition: imu_chimu.h:67
#define CHIMU_STX
Definition: imu_chimu.h:47
void ahrs_init(void)
AHRS initialization.
euler angles
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
Definition: imu_chimu.c:83
void parse_ins_msg(void)
#define MSG00_PING
Definition: imu_chimu.h:51
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
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
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
void ahrs_align(void)
Aligns the AHRS.
#define InsLink(_x)
Definition: ins_module.h:79
#define INS_FORMAT
Definition: ins_module.h:35
#define AHRS_UNINIT
Definition: ahrs.h:35
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
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
CHIMU_PARSER_DATA CHIMU_DATA
float theta
Definition: imu_chimu.h:120
arch independent LED (Light Emitting Diodes) API
#define MSG09_ESTIMATOR
Definition: imu_chimu.h:60
#define InsUartSend1(c)
Definition: ins_module.h:84
Device independent INS code.
unsigned char m_MsgID
Definition: imu_chimu.h:163
#define AHRS_RUNNING
Definition: ahrs.h:36
INS_FORMAT ins_pitch_neutral