-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
ins_arduimu_basic.c
184 lines (151 loc) · 6.11 KB
/
ins_arduimu_basic.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
/*
C Datei für die Einbindung eines ArduIMU's
Autoren@ZHAW: schmiemi
chaneren
*/
#include <stdbool.h>
#include "modules/ins/ins_arduimu_basic.h"
//#include "firmwares/fixedwing/main_fbw.h"
#include "mcu_periph/i2c.h"
// test
#include "estimator.h"
// GPS data for ArduIMU
#ifndef UBX
#error "currently only compatible with uBlox GPS modules"
#endif
#include "subsystems/gps.h"
// Command vector for thrust
#include "generated/airframe.h"
#include "inter_mcu.h"
#define NB_DATA 9
#ifndef ARDUIMU_I2C_DEV
#define ARDUIMU_I2C_DEV i2c0
#endif
// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
// einzugebende Adresse im ArduIMU ist 0000 1011
// da ArduIMU das Read/Write Bit selber anfügt.
#define ArduIMU_SLAVE_ADDR 0x22
#ifdef ARDUIMU_SYNC_SEND
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#endif
struct i2c_transaction ardu_gps_trans;
struct i2c_transaction ardu_ins_trans;
static int16_t recievedData[NB_DATA];
struct FloatEulers arduimu_eulers;
struct FloatRates arduimu_rates;
struct FloatVect3 arduimu_accel;
float ins_roll_neutral;
float ins_pitch_neutral;
// High Accel Flag
#define HIGH_ACCEL_LOW_SPEED 15.0
#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
bool_t high_accel_done;
bool_t high_accel_flag;
void ArduIMU_init( void ) {
FLOAT_EULERS_ZERO(arduimu_eulers);
FLOAT_RATES_ZERO(arduimu_rates);
FLOAT_VECT3_ZERO(arduimu_accel);
ardu_ins_trans.status = I2CTransDone;
ardu_gps_trans.status = I2CTransDone;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
high_accel_done = FALSE;
high_accel_flag = FALSE;
}
#define FillBufWith32bit(_buf, _index, _value) { \
_buf[_index] = (uint8_t) (_value); \
_buf[_index+1] = (uint8_t) ((_value) >> 8); \
_buf[_index+2] = (uint8_t) ((_value) >> 16); \
_buf[_index+3] = (uint8_t) ((_value) >> 24); \
}
void ArduIMU_periodicGPS( void ) {
if (ardu_gps_trans.status != I2CTransDone) { return; }
// Test for high acceleration:
// - low speed
// - high thrust
if (estimator_hspeed_dir < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (estimator_hspeed_dir > HIGH_ACCEL_LOW_SPEED && !high_accel_flag) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (estimator_hspeed_dir < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course
ardu_gps_trans.buf[12] = gps.fix; // status gps fix
ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags
ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
}
void ArduIMU_periodic( void ) {
//Frequence defined in conf/modules/ins_arduimu.xml
if (ardu_ins_trans.status == I2CTransDone) {
I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2);
}
}
#include "math/pprz_algebra_int.h"
/*
Buffer O: Roll
Buffer 1: Pitch
Buffer 2: Yaw
Buffer 3: Gyro X
Buffer 4: Gyro Y
Buffer 5: Gyro Z
Buffer 6: Accel X
Buffer 7: Accel Y
Buffer 8: Accel Z
*/
void ArduIMU_event( void ) {
// Handle INS I2C event
if (ardu_ins_trans.status == I2CTransSuccess) {
// received data from I2C transaction
recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
estimator_p = arduimu_rates.p;
ardu_ins_trans.status = I2CTransDone;
#ifdef ARDUIMU_SYNC_SEND
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
#endif
}
else if (ardu_ins_trans.status == I2CTransFailed) {
ardu_ins_trans.status = I2CTransDone;
}
// Handle GPS I2C event
if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
ardu_gps_trans.status = I2CTransDone;
}
}