Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_dcm.c
Go to the documentation of this file.
1/*
2 * Released under Creative Commons License
3 *
4 * 2010 The Paparazzi Team
5 *
6 *
7 * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
8 * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
9 *
10 * Modified at Hochschule Bremen, Germany
11 * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
12 *
13 */
14
28#include "std.h"
29
32#include "autopilot.h" // launch detection
33
36
37#if USE_GPS
38#include "modules/gps/gps.h"
39#endif
40
41#include <string.h>
42
43#include "led.h"
44
45#if FLOAT_DCM_SEND_DEBUG
46// FIXME Debugging Only
47#include "mcu_periph/uart.h"
48#include "pprzlink/messages.h"
50#endif
51
53
54// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
55// Positive pitch : nose up
56// Positive roll : right wing down
57// Positive yaw : clockwise
58
59struct FloatVect3 accel_float = {0, 0, 0};
60struct FloatVect3 mag_float = {0, 0, 0};
61
62float Omega_Vector[3] = {0, 0, 0}; //Corrected Gyro_Vector data
63float Omega_P[3] = {0, 0, 0}; //Omega Proportional correction
64float Omega_I[3] = {0, 0, 0}; //Omega Integrator
65float Omega[3] = {0, 0, 0};
66
67float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
68float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; //Gyros here
69float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
70
71#if USE_MAGNETOMETER
72float MAG_Heading_X = 1;
73float MAG_Heading_Y = 0;
74#endif
75
76static void compute_ahrs_representations(void);
77static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
78
79void Normalize(void);
80void Drift_correction(void);
81void Matrix_update(float dt);
82
83#if PERFORMANCE_REPORTING == 1
84int renorm_sqrt_count = 0;
86float imu_health = 0.;
87#endif
88
89
90static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
91{
92 for (int i = 0; i < 3; i++) {
93 for (int j = 0; j < 3; j++) {
94 DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
95 }
96 }
97}
98
99void ahrs_dcm_init(void)
100{
102 ahrs_dcm.is_aligned = false;
103
104 /* init ltp_to_body euler with zero */
107
108 /* set inital filter dcm */
109 struct FloatRMat init_rmat;
112
117 ahrs_dcm.gps_age = 100;
118}
119
120bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
121 struct FloatVect3 *lp_mag __attribute__((unused)))
122{
123 /* Compute an initial orientation in quaternion (and then back to euler angles) so it works upside-down as well */
124 struct FloatQuat quat;
125#if USE_MAGNETOMETER
126 ahrs_float_get_quat_from_accel_mag(&quat, lp_accel, lp_mag);
127#else
128 ahrs_float_get_quat_from_accel(&quat, lp_accel);
129#endif
131
132 /* Convert initial orientation from quaternion to rotation matrix representations. */
133 struct FloatRMat ltp_to_body_rmat;
134 float_rmat_of_quat(&ltp_to_body_rmat, &quat);
135
136 /* set filter dcm */
137 set_dcm_matrix_from_rmat(&ltp_to_body_rmat);
138
139 /* use averaged gyro as initial value for bias */
140 ahrs_dcm.gyro_bias = *lp_gyro;
141
143 ahrs_dcm.is_aligned = true;
144
145 return true;
146}
147
148
149void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
150{
151 /* unbias rate measurement */
153
154 /* Uncouple Motions */
155#ifdef IMU_GYRO_P_Q
156 float dp = 0, dq = 0, dr = 0;
163
164 ahrs_dcm.body_rate.p += dp;
167#endif
168
169 Matrix_update(dt);
170
171 Normalize();
172
174}
175
177{
178 static float last_gps_speed_3d = 0;
179
180#if USE_GPS
181 if (gps_s->fix >= GPS_FIX_3D) {
182 ahrs_dcm.gps_age = 0;
183 ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
184
185 if (gps_s->gspeed >= 100*AHRS_FLOAT_MIN_SPEED_GPS_COURSE) { // AHRS_FLOAT_MIN_SPEED_GPS_COURSE m/s
186 ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
188 } else {
190 }
191 } else {
192 ahrs_dcm.gps_age = 100;
193 }
194#endif
195
198}
199
200
202{
203 // DCM filter uses g-force as positive
204 // accelerometer measures [0 0 -g] in a static case
205 accel_float.x = -accel->x;
206 accel_float.y = -accel->y;
207 accel_float.z = -accel->z;
208
209
210 ahrs_dcm.gps_age ++;
211 if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
212#if USE_AHRS_GPS_ACCELERATIONS
213 PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
214 accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
215#endif
216 accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
217 accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
218 } else {
221 ahrs_dcm.gps_age = 100;
222 }
223
225}
226
227
229{
230#if USE_MAGNETOMETER
231
232 float cos_roll;
233 float sin_roll;
234 float cos_pitch;
235 float sin_pitch;
236
241
242
243 // Pitch&Roll Compensation:
244 MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
245 MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;
246
247 /*
248 *
249 // Magnetic Heading
250 Heading = atan2(-Head_Y,Head_X);
251
252 // Declination correction (if supplied)
253 if( declination != 0.0 )
254 {
255 Heading = Heading + declination;
256 if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
257 Heading -= (2.0 * M_PI);
258 else if (Heading < -M_PI)
259 Heading += (2.0 * M_PI);
260 }
261
262 // Optimization for external DCM use. Calculate normalized components
263 Heading_X = cos(Heading);
264 Heading_Y = sin(Heading);
265 */
266
267#if FLOAT_DCM_SEND_DEBUG
268 struct FloatVect3 ltp_mag;
269
272
273 // Downlink
275#endif
276
277 // Magnetic Heading
278 // MAG_Heading = atan2(mag->y, -mag->x);
279
280#else // !USE_MAGNETOMETER
281 // get rid of unused param warning...
282 mag = mag;
283#endif
284}
285
286void Normalize(void)
287{
288 float error = 0;
289 float temporary[3][3];
290 float renorm = 0;
291 uint8_t problem = false;
292
293 // Find the non-orthogonality of X wrt Y
294 error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
295
296 // Add half the XY error to X, and half to Y
297 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
298 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
299 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
300 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
301
302 // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
303 Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
304
305 // Normalize lenght of X
307 // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
308 // b) if the norm is further from 1, use a real sqrt
309 // c) norm is huge: disaster! reset! mayday!
311 renorm = .5 * (3 - renorm); //eq.21
312 } else if (renorm < 100.0f && renorm > 0.01f) {
313 renorm = 1. / sqrtf(renorm);
314#if PERFORMANCE_REPORTING == 1
316#endif
317 } else {
318 problem = true;
319#if PERFORMANCE_REPORTING == 1
321#endif
322 }
323 Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
324
325 // Normalize lenght of Y
328 renorm = .5 * (3 - renorm); //eq.21
329 } else if (renorm < 100.0f && renorm > 0.01f) {
330 renorm = 1. / sqrtf(renorm);
331#if PERFORMANCE_REPORTING == 1
333#endif
334 } else {
335 problem = true;
336#if PERFORMANCE_REPORTING == 1
338#endif
339 }
340 Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
341
342 // Normalize lenght of Z
345 renorm = .5 * (3 - renorm); //eq.21
346 } else if (renorm < 100.0f && renorm > 0.01f) {
347 renorm = 1. / sqrtf(renorm);
348#if PERFORMANCE_REPORTING == 1
350#endif
351 } else {
352 problem = true;
353#if PERFORMANCE_REPORTING == 1
355#endif
356 }
357 Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
358
359 // Reset on trouble
360 if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
361 struct FloatRMat init_rmat;
364 problem = false;
365 }
366}
367
368// strong structural vibrations can prevent to perform the drift correction
369// so accel magnitude is filtered before computing the weighting heuristic
370#ifndef ACCEL_WEIGHT_FILTER
371#define ACCEL_WEIGHT_FILTER 8
372#endif
373
374// the weigthing is function of the length of a band of 1G by default
375// so <0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0
376// adjust the band size if needed, the value should be >0
377#ifndef ACCEL_WEIGHT_BAND
378#define ACCEL_WEIGHT_BAND 1.f
379#endif
380
382{
383 //Compensation the Roll, Pitch and Yaw drift.
384 static float Scaled_Omega_P[3];
385 static float Scaled_Omega_I[3];
386 static float Accel_filtered = 0.f;
387 float Accel_magnitude;
388 float Accel_weight;
390
391 // Local Working Variables
392 float errorRollPitch[3];
393 float errorYaw[3];
394 float errorCourse;
395
396 //*****Roll and Pitch***************
397
398 // Calculate the magnitude of the accelerometer vector
400 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
401#if ACCEL_WEIGHT_FILTER
403#else // set ACCEL_WEIGHT_FILTER to 0 to disable filter
405#endif
406 // Dynamic weighting of accelerometer info (reliability filter)
407 // Weight for accelerometer info according to band size (min value is 0.1 to prevent division by zero)
408 Accel_weight = Clip(1.f - (2.f / Max(0.1f,ACCEL_WEIGHT_BAND)) * fabsf(1.f - Accel_filtered), 0.f, 1.f);
409
410
411#if PERFORMANCE_REPORTING == 1
412 {
413 //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
414 float tempfloat = ((Accel_weight - 0.5) * 256.0f);
416 Bound(imu_health, 129, 65405);
417 }
418#endif
419
420 Vector_Cross_Product(&errorRollPitch[0], (float *)&accel_float, &DCM_Matrix[2][0]); //adjust the ground of reference
422
425
426 //*****YAW***************
427
428#if USE_MAGNETOMETER
429 // We make the gyro YAW drift correction based on compass magnetic heading
430// float mag_heading_x = cos(MAG_Heading);
431// float mag_heading_y = sin(MAG_Heading);
432 // 2D dot product
433 //Calculating YAW error
435 //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
437
439 Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
440
442 Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
443
444#else // Use GPS Ground course to correct yaw gyro drift
445
447 float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
448 float COGX = cosf(course); //Course overground X axis
449 float COGY = sinf(course); //Course overground Y axis
450
451 errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
452 //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
454
456 Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
457
459 Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
460 }
461#if USE_MAGNETOMETER_ONGROUND
462 PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
463 else if (autopilot.launch == FALSE) {
464 float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
465 float COGY = mag->y; // Non-Tilt-Compensated (for filter stability reasons)
466
467 errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
468 //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
470
471 // P only
472 Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
473 Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
474 }
475#endif // USE_MAGNETOMETER_ONGROUND
476#endif
477
478 // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
480 if (Integrator_magnitude > RadOfDeg(300)) {
482 }
483
484
485}
486/**************************************************/
487
488void Matrix_update(float dt)
489{
490 Vector_Add(&Omega[0], (float *)&ahrs_dcm.body_rate, &Omega_I[0]); //adding proportional term
491 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
492
493#if OUTPUTMODE==1 // With corrected data (drift correction)
494 Update_Matrix[0][0] = 0;
495 Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
496 Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
497 Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
498 Update_Matrix[1][1] = 0;
499 Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
500 Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
501 Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
502 Update_Matrix[2][2] = 0;
503#else // Uncorrected data (no drift correction)
504 Update_Matrix[0][0] = 0;
505 Update_Matrix[0][1] = -dt * ahrs_dcm.body_rate.r; //-z
506 Update_Matrix[0][2] = dt * ahrs_dcm.body_rate.q; //y
507 Update_Matrix[1][0] = dt * ahrs_dcm.body_rate.r; //z
508 Update_Matrix[1][1] = 0;
509 Update_Matrix[1][2] = -dt * ahrs_dcm.body_rate.p;
510 Update_Matrix[2][0] = -dt * ahrs_dcm.body_rate.q;
511 Update_Matrix[2][1] = dt * ahrs_dcm.body_rate.p;
512 Update_Matrix[2][2] = 0;
513#endif
514
516
517 for (int x = 0; x < 3; x++) { //Matrix Addition (update)
518 for (int y = 0; y < 3; y++) {
519 DCM_Matrix[x][y] += Temporary_Matrix[x][y];
520 }
521 }
522}
523
525{
526#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
527 ahrs_dcm.ltp_to_body_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
530#else
534#endif
535}
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
void Drift_correction(void)
void ahrs_dcm_update_gps(struct GpsState *gps_s UNUSED)
float Omega_Vector[3]
void Normalize(void)
float DCM_Matrix[3][3]
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
float Omega_I[3]
float MAG_Heading_Y
float MAG_Heading_X
static void compute_ahrs_representations(void)
float Temporary_Matrix[3][3]
void ahrs_dcm_init(void)
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
void Matrix_update(float dt)
static void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
#define ACCEL_WEIGHT_FILTER
struct AhrsFloatDCM ahrs_dcm
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
#define ACCEL_WEIGHT_BAND
float Omega[3]
float Update_Matrix[3][3]
float Omega_P[3]
struct FloatVect3 accel_float
struct FloatVect3 mag_float
Attitude estimation for fixedwings based on the DCM.
#define Kp_ROLLPITCH
#define Kp_YAW
#define AHRS_FLOAT_MIN_SPEED_GPS_COURSE
#define GRAVITY
float gps_acceleration
#define Ki_ROLLPITCH
struct FloatRates body_rate
struct FloatEulers ltp_to_body_euler
struct FloatRates gyro_bias
@ AHRS_DCM_UNINIT
@ AHRS_DCM_RUNNING
#define Ki_YAW
enum AhrsDCMStatus status
Algebra helper functions for DCM.
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
static float Vector_Dot_Product(float vector1[3], float vector2[3])
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
static int16_t course[3]
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:49
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition autopilot.h:71
#define UNUSED(x)
Device independent GPS code (interface)
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:87
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
#define FLOAT_EULERS_ZERO(_e)
static void float_rmat_identity(struct FloatRMat *rm)
initialises a rotation matrix to identity
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define FLOAT_RATES_ZERO(_r)
Roation quaternion.
rotation matrix
angular rates
#define RMAT_ELMT(_rm, _row, _col)
#define RATES_DIFF(_c, _a, _b)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
arch independent LED (Light Emitting Diodes) API
uint16_t foo
Definition main_demo5.c:58
Paparazzi floating point algebra.
#define FALSE
Definition std.h:5
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.