Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
52 struct AhrsFloatDCM ahrs_dcm;
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 
59 struct FloatVect3 accel_float = {0, 0, 0};
60 struct FloatVect3 mag_float = {0, 0, 0};
61 
62 float Omega_Vector[3] = {0, 0, 0}; //Corrected Gyro_Vector data
63 float Omega_P[3] = {0, 0, 0}; //Omega Proportional correction
64 float Omega_I[3] = {0, 0, 0}; //Omega Integrator
65 float Omega[3] = {0, 0, 0};
66 
67 float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
68 float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; //Gyros here
69 float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
70 
71 #if USE_MAGNETOMETER
72 float MAG_Heading_X = 1;
73 float MAG_Heading_Y = 0;
74 #endif
75 
76 static void compute_ahrs_representations(void);
77 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
78 
79 void Normalize(void);
80 void Drift_correction(void);
81 void Matrix_update(float dt);
82 
83 #if PERFORMANCE_REPORTING == 1
84 int renorm_sqrt_count = 0;
85 int renorm_blowup_count = 0;
86 float imu_health = 0.;
87 #endif
88 
89 
90 static 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 
99 void 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;
110  float_rmat_identity(&init_rmat);
111  set_dcm_matrix_from_rmat(&init_rmat);
112 
113  ahrs_dcm.gps_speed = 0;
115  ahrs_dcm.gps_course = 0;
116  ahrs_dcm.gps_course_valid = false;
117  ahrs_dcm.gps_age = 100;
118 }
119 
120 bool 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 
149 void 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;
157  dp += ahrs_dcm.body_rate.q * IMU_GYRO_P_Q;
158  dp += ahrs_dcm.body_rate.r * IMU_GYRO_P_R;
159  dq += ahrs_dcm.body_rate.p * IMU_GYRO_Q_P;
160  dq += ahrs_dcm.body_rate.r * IMU_GYRO_Q_R;
161  dr += ahrs_dcm.body_rate.p * IMU_GYRO_R_P;
162  dr += ahrs_dcm.body_rate.q * IMU_GYRO_R_Q;
163 
164  ahrs_dcm.body_rate.p += dp;
165  ahrs_dcm.body_rate.q += dq;
166  ahrs_dcm.body_rate.r += dr;
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;
187  ahrs_dcm.gps_course_valid = true;
188  } else {
189  ahrs_dcm.gps_course_valid = false;
190  }
191  } else {
192  ahrs_dcm.gps_age = 100;
193  }
194 #endif
195 
196  ahrs_dcm.gps_acceleration += (((ahrs_dcm.gps_speed - last_gps_speed_3d) * 4.0f) - ahrs_dcm.gps_acceleration) / 5.0f;
197  last_gps_speed_3d = ahrs_dcm.gps_speed;
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 {
219  ahrs_dcm.gps_speed = 0;
221  ahrs_dcm.gps_age = 100;
222  }
223 
225 }
226 
227 
229 {
230 #if USE_MAGNETOMETER
231 MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
232 
233  float cos_roll;
234  float sin_roll;
235  float cos_pitch;
236  float sin_pitch;
237 
238  cos_roll = cosf(ahrs_dcm.ltp_to_body_euler.phi);
239  sin_roll = sinf(ahrs_dcm.ltp_to_body_euler.phi);
240  cos_pitch = cosf(ahrs_dcm.ltp_to_body_euler.theta);
241  sin_pitch = sinf(ahrs_dcm.ltp_to_body_euler.theta);
242 
243 
244  // Pitch&Roll Compensation:
245  MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
246  MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;
247 
248  /*
249  *
250  // Magnetic Heading
251  Heading = atan2(-Head_Y,Head_X);
252 
253  // Declination correction (if supplied)
254  if( declination != 0.0 )
255  {
256  Heading = Heading + declination;
257  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
258  Heading -= (2.0 * M_PI);
259  else if (Heading < -M_PI)
260  Heading += (2.0 * M_PI);
261  }
262 
263  // Optimization for external DCM use. Calculate normalized components
264  Heading_X = cos(Heading);
265  Heading_Y = sin(Heading);
266  */
267 
268 #if FLOAT_DCM_SEND_DEBUG
269  struct FloatVect3 ltp_mag;
270 
271  ltp_mag.x = MAG_Heading_X;
272  ltp_mag.y = MAG_Heading_Y;
273 
274  // Downlink
275  RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
276 #endif
277 
278  // Magnetic Heading
279  // MAG_Heading = atan2(mag->y, -mag->x);
280 
281 #else // !USE_MAGNETOMETER
282  // get rid of unused param warning...
283  mag = mag;
284 #endif
285 }
286 
287 void Normalize(void)
288 {
289  float error = 0;
290  float temporary[3][3];
291  float renorm = 0;
292  uint8_t problem = false;
293 
294  // Find the non-orthogonality of X wrt Y
295  error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
296 
297  // Add half the XY error to X, and half to Y
298  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
299  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
300  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
301  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
302 
303  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
304  Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
305 
306  // Normalize lenght of X
307  renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
308  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
309  // b) if the norm is further from 1, use a real sqrt
310  // c) norm is huge: disaster! reset! mayday!
311  if (renorm < 1.5625f && renorm > 0.64f) {
312  renorm = .5 * (3 - renorm); //eq.21
313  } else if (renorm < 100.0f && renorm > 0.01f) {
314  renorm = 1. / sqrtf(renorm);
315 #if PERFORMANCE_REPORTING == 1
317 #endif
318  } else {
319  problem = true;
320 #if PERFORMANCE_REPORTING == 1
322 #endif
323  }
324  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
325 
326  // Normalize lenght of Y
327  renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
328  if (renorm < 1.5625f && renorm > 0.64f) {
329  renorm = .5 * (3 - renorm); //eq.21
330  } else if (renorm < 100.0f && renorm > 0.01f) {
331  renorm = 1. / sqrtf(renorm);
332 #if PERFORMANCE_REPORTING == 1
334 #endif
335  } else {
336  problem = true;
337 #if PERFORMANCE_REPORTING == 1
339 #endif
340  }
341  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
342 
343  // Normalize lenght of Z
344  renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
345  if (renorm < 1.5625f && renorm > 0.64f) {
346  renorm = .5 * (3 - renorm); //eq.21
347  } else if (renorm < 100.0f && renorm > 0.01f) {
348  renorm = 1. / sqrtf(renorm);
349 #if PERFORMANCE_REPORTING == 1
351 #endif
352  } else {
353  problem = true;
354 #if PERFORMANCE_REPORTING == 1
356 #endif
357  }
358  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
359 
360  // Reset on trouble
361  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
362  struct FloatRMat init_rmat;
363  float_rmat_identity(&init_rmat);
364  set_dcm_matrix_from_rmat(&init_rmat);
365  problem = false;
366  }
367 }
368 
369 // strong structural vibrations can prevent to perform the drift correction
370 // so accel magnitude is filtered before computing the weighting heuristic
371 #ifndef ACCEL_WEIGHT_FILTER
372 #define ACCEL_WEIGHT_FILTER 8
373 #endif
374 
375 // the weigthing is function of the length of a band of 1G by default
376 // so <0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0
377 // adjust the band size if needed, the value should be >0
378 #ifndef ACCEL_WEIGHT_BAND
379 #define ACCEL_WEIGHT_BAND 1.f
380 #endif
381 
383 {
384  //Compensation the Roll, Pitch and Yaw drift.
385  static float Scaled_Omega_P[3];
386  static float Scaled_Omega_I[3];
387  static float Accel_filtered = 0.f;
388  float Accel_magnitude;
389  float Accel_weight;
390  float Integrator_magnitude;
391 
392  // Local Working Variables
393  float errorRollPitch[3];
394  float errorYaw[3];
395  float errorCourse;
396 
397  //*****Roll and Pitch***************
398 
399  // Calculate the magnitude of the accelerometer vector
400  Accel_magnitude = sqrtf(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
401  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
402 #if ACCEL_WEIGHT_FILTER
403  Accel_filtered = (Accel_magnitude + (ACCEL_WEIGHT_FILTER - 1) * Accel_filtered) / ACCEL_WEIGHT_FILTER;
404 #else // set ACCEL_WEIGHT_FILTER to 0 to disable filter
405  Accel_filtered = Accel_magnitude;
406 #endif
407  // Dynamic weighting of accelerometer info (reliability filter)
408  // Weight for accelerometer info according to band size (min value is 0.1 to prevent division by zero)
409  Accel_weight = Clip(1.f - (2.f / Max(0.1f,ACCEL_WEIGHT_BAND)) * fabsf(1.f - Accel_filtered), 0.f, 1.f);
410 
411 
412 #if PERFORMANCE_REPORTING == 1
413  {
414  //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
415  float tempfloat = ((Accel_weight - 0.5) * 256.0f);
416  imu_health += tempfloat;
417  Bound(imu_health, 129, 65405);
418  }
419 #endif
420 
421  Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
422  Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
423 
424  Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
425  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
426 
427  //*****YAW***************
428 
429 #if USE_MAGNETOMETER
430  // We make the gyro YAW drift correction based on compass magnetic heading
431 // float mag_heading_x = cos(MAG_Heading);
432 // float mag_heading_y = sin(MAG_Heading);
433  // 2D dot product
434  //Calculating YAW error
435  errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
436  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
437  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
438 
439  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
440  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
441 
442  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
443  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
444 
445 #else // Use GPS Ground course to correct yaw gyro drift
446 
448  float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
449  float COGX = cosf(course); //Course overground X axis
450  float COGY = sinf(course); //Course overground Y axis
451 
452  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
453  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
454  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
455 
456  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
457  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
458 
459  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
460  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
461  }
462 #if USE_MAGNETOMETER_ONGROUND
463  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
464  else if (autopilot.launch == FALSE) {
465  float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
466  float COGY = mag->y; // Non-Tilt-Compensated (for filter stability reasons)
467 
468  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
469  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
470  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
471 
472  // P only
473  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
474  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
475  }
476 #endif // USE_MAGNETOMETER_ONGROUND
477 #endif
478 
479  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
480  Integrator_magnitude = sqrtf(Vector_Dot_Product(Omega_I, Omega_I));
481  if (Integrator_magnitude > RadOfDeg(300)) {
482  Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
483  }
484 
485 
486 }
487 /**************************************************/
488 
489 void Matrix_update(float dt)
490 {
491  Vector_Add(&Omega[0], &ahrs_dcm.body_rate.p, &Omega_I[0]); //adding proportional term
492  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
493 
494 #if OUTPUTMODE==1 // With corrected data (drift correction)
495  Update_Matrix[0][0] = 0;
496  Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
497  Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
498  Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
499  Update_Matrix[1][1] = 0;
500  Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
501  Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
502  Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
503  Update_Matrix[2][2] = 0;
504 #else // Uncorrected data (no drift correction)
505  Update_Matrix[0][0] = 0;
506  Update_Matrix[0][1] = -dt * ahrs_dcm.body_rate.r; //-z
507  Update_Matrix[0][2] = dt * ahrs_dcm.body_rate.q; //y
508  Update_Matrix[1][0] = dt * ahrs_dcm.body_rate.r; //z
509  Update_Matrix[1][1] = 0;
510  Update_Matrix[1][2] = -dt * ahrs_dcm.body_rate.p;
511  Update_Matrix[2][0] = -dt * ahrs_dcm.body_rate.q;
512  Update_Matrix[2][1] = dt * ahrs_dcm.body_rate.p;
513  Update_Matrix[2][2] = 0;
514 #endif
515 
517 
518  for (int x = 0; x < 3; x++) { //Matrix Addition (update)
519  for (int y = 0; y < 3; y++) {
520  DCM_Matrix[x][y] += Temporary_Matrix[x][y];
521  }
522  }
523 }
524 
526 {
527 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
528  ahrs_dcm.ltp_to_body_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
529  ahrs_dcm.ltp_to_body_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
531 #else
532  ahrs_dcm.ltp_to_body_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
534  ahrs_dcm.ltp_to_body_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
535  ahrs_dcm.ltp_to_body_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
536 #endif
537 }
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.
float imu_health
bool gps_course_valid
int renorm_sqrt_count
#define Kp_ROLLPITCH
#define Kp_YAW
#define AHRS_FLOAT_MIN_SPEED_GPS_COURSE
#define GRAVITY
float gps_acceleration
#define Ki_ROLLPITCH
uint8_t gps_age
struct FloatRates body_rate
struct FloatEulers ltp_to_body_euler
struct FloatRates gyro_bias
int renorm_blowup_count
@ 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]
Definition: airspeed_uADC.c:58
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
Device independent GPS code (interface)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
data structure for GPS information
Definition: gps.h:90
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)
Definition: pprz_algebra.h:660
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
arch independent LED (Light Emitting Diodes) API
Paparazzi floating point algebra.
uint8_t last_wp UNUSED
Definition: navigation.c:101
#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.
Definition: vl53l1_types.h:98