Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 "firmwares/fixedwing/autopilot.h" // launch detection
33 
36 
37 #if USE_GPS
38 #include "subsystems/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 "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 
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 {
103 
104  /* init ltp_to_imu euler with zero */
106 
108 
109  /* set inital filter dcm */
111 
112  ahrs_dcm.gps_speed = 0;
114  ahrs_dcm.gps_course = 0;
116  ahrs_dcm.gps_age = 100;
117 }
118 
119 bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
120  struct Int32Vect3 *lp_mag)
121 {
122  /* Compute an initial orientation using euler angles */
124 
125  /* Convert initial orientation in quaternion and rotation matrice representations. */
126  struct FloatRMat ltp_to_imu_rmat;
127  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
128 
129  /* set filter dcm */
130  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
131 
132  /* use averaged gyro as initial value for bias */
133  struct Int32Rates bias0;
134  RATES_COPY(bias0, *lp_gyro);
136 
139 
140  return TRUE;
141 }
142 
143 
144 void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
145 {
146  /* convert imu data to floating point */
147  struct FloatRates gyro_float;
148  RATES_FLOAT_OF_BFP(gyro_float, *gyro);
149 
150  /* unbias rate measurement */
152 
153  /* Uncouple Motions */
154 #ifdef IMU_GYRO_P_Q
155  float dp = 0, dq = 0, dr = 0;
156  dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
157  dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
158  dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
159  dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
160  dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
161  dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
162 
163  ahrs_dcm.imu_rate.p += dp;
164  ahrs_dcm.imu_rate.q += dq;
165  ahrs_dcm.imu_rate.r += dr;
166 #endif
167 
168  Matrix_update(dt);
169 
170  Normalize();
171 
173 }
174 
175 void ahrs_dcm_update_gps(struct GpsState *gps_s)
176 {
177  static float last_gps_speed_3d = 0;
178 
179 #if USE_GPS
180  if (gps_s->fix >= GPS_FIX_3D) {
181  ahrs_dcm.gps_age = 0;
182  ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
183 
184  if (gps_s->gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s
185  ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
187  } else {
189  }
190  } else {
191  ahrs_dcm.gps_age = 100;
192  }
193 #endif
194 
195  ahrs_dcm.gps_acceleration += (((ahrs_dcm.gps_speed - last_gps_speed_3d) * 4.0f) - ahrs_dcm.gps_acceleration) / 5.0f;
196  last_gps_speed_3d = ahrs_dcm.gps_speed;
197 }
198 
199 
201 {
202  ACCELS_FLOAT_OF_BFP(accel_float, *accel);
203 
204  // DCM filter uses g-force as positive
205  // accelerometer measures [0 0 -g] in a static case
206  accel_float.x = -accel_float.x;
207  accel_float.y = -accel_float.y;
208  accel_float.z = -accel_float.z;
209 
210 
211  ahrs_dcm.gps_age ++;
212  if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
213 #if USE_AHRS_GPS_ACCELERATIONS
214  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
215  accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
216 #endif
217  accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
218  accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
219  } else {
220  ahrs_dcm.gps_speed = 0;
222  ahrs_dcm.gps_age = 100;
223  }
224 
226 }
227 
228 
230 {
231 #if USE_MAGNETOMETER
232 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
233 
234  MAG_FLOAT_OF_BFP(mag_float, *mag);
235 
236  float cos_roll;
237  float sin_roll;
238  float cos_pitch;
239  float sin_pitch;
240 
241  cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
242  sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
243  cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
244  sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
245 
246 
247  // Pitch&Roll Compensation:
248  MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
249  MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;
250 
251  /*
252  *
253  // Magnetic Heading
254  Heading = atan2(-Head_Y,Head_X);
255 
256  // Declination correction (if supplied)
257  if( declination != 0.0 )
258  {
259  Heading = Heading + declination;
260  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
261  Heading -= (2.0 * M_PI);
262  else if (Heading < -M_PI)
263  Heading += (2.0 * M_PI);
264  }
265 
266  // Optimization for external DCM use. Calculate normalized components
267  Heading_X = cos(Heading);
268  Heading_Y = sin(Heading);
269  */
270 
271  struct FloatVect3 ltp_mag;
272 
273  ltp_mag.x = MAG_Heading_X;
274  ltp_mag.y = MAG_Heading_Y;
275 
276 #if FLOAT_DCM_SEND_DEBUG
277  // Downlink
278  RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
279 #endif
280 
281  // Magnetic Heading
282  // MAG_Heading = atan2(mag->y, -mag->x);
283 
284 #else // !USE_MAGNETOMETER
285  // get rid of unused param warning...
286  mag = mag;
287 #endif
288 }
289 
290 void Normalize(void)
291 {
292  float error = 0;
293  float temporary[3][3];
294  float renorm = 0;
295  uint8_t problem = FALSE;
296 
297  // Find the non-orthogonality of X wrt Y
298  error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
299 
300  // Add half the XY error to X, and half to Y
301  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
302  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
303  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
304  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
305 
306  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
307  Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
308 
309  // Normalize lenght of X
310  renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
311  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
312  // b) if the norm is further from 1, use a real sqrt
313  // c) norm is huge: disaster! reset! mayday!
314  if (renorm < 1.5625f && renorm > 0.64f) {
315  renorm = .5 * (3 - renorm); //eq.21
316  } else if (renorm < 100.0f && renorm > 0.01f) {
317  renorm = 1. / sqrt(renorm);
318 #if PERFORMANCE_REPORTING == 1
320 #endif
321  } else {
322  problem = TRUE;
323 #if PERFORMANCE_REPORTING == 1
325 #endif
326  }
327  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
328 
329  // Normalize lenght of Y
330  renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
331  if (renorm < 1.5625f && renorm > 0.64f) {
332  renorm = .5 * (3 - renorm); //eq.21
333  } else if (renorm < 100.0f && renorm > 0.01f) {
334  renorm = 1. / sqrt(renorm);
335 #if PERFORMANCE_REPORTING == 1
337 #endif
338  } else {
339  problem = TRUE;
340 #if PERFORMANCE_REPORTING == 1
342 #endif
343  }
344  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
345 
346  // Normalize lenght of Z
347  renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
348  if (renorm < 1.5625f && renorm > 0.64f) {
349  renorm = .5 * (3 - renorm); //eq.21
350  } else if (renorm < 100.0f && renorm > 0.01f) {
351  renorm = 1. / sqrt(renorm);
352 #if PERFORMANCE_REPORTING == 1
354 #endif
355  } else {
356  problem = TRUE;
357 #if PERFORMANCE_REPORTING == 1
359 #endif
360  }
361  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
362 
363  // Reset on trouble
364  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
366  problem = FALSE;
367  }
368 }
369 
370 
372 {
373  //Compensation the Roll, Pitch and Yaw drift.
374  static float Scaled_Omega_P[3];
375  static float Scaled_Omega_I[3];
376  float Accel_magnitude;
377  float Accel_weight;
378  float Integrator_magnitude;
379 
380  // Local Working Variables
381  float errorRollPitch[3];
382  float errorYaw[3];
383  float errorCourse;
384 
385  //*****Roll and Pitch***************
386 
387  // Calculate the magnitude of the accelerometer vector
388  Accel_magnitude = sqrt(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
389  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
390  // Dynamic weighting of accelerometer info (reliability filter)
391  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
392  Accel_weight = Chop(1 - 2 * fabs(1 - Accel_magnitude), 0, 1); //
393 
394 
395 #if PERFORMANCE_REPORTING == 1
396  {
397  //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
398  float tempfloat = ((Accel_weight - 0.5) * 256.0f);
399  imu_health += tempfloat;
400  Bound(imu_health, 129, 65405);
401  }
402 #endif
403 
404  Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
405  Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
406 
407  Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
408  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
409 
410  //*****YAW***************
411 
412 #if USE_MAGNETOMETER
413  // We make the gyro YAW drift correction based on compass magnetic heading
414 // float mag_heading_x = cos(MAG_Heading);
415 // float mag_heading_y = sin(MAG_Heading);
416  // 2D dot product
417  //Calculating YAW error
418  errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
419  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
420  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
421 
422  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
423  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
424 
425  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
426  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
427 
428 #else // Use GPS Ground course to correct yaw gyro drift
429 
431  float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
432  float COGX = cosf(course); //Course overground X axis
433  float COGY = sinf(course); //Course overground Y axis
434 
435  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
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 #if USE_MAGNETOMETER_ONGROUND == 1
446  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
447  else if (launch == FALSE) {
448  float COGX = mag_float.x; // Non-Tilt-Compensated (for filter stability reasons)
449  float COGY = mag_float.y; // Non-Tilt-Compensated (for filter stability reasons)
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.
453  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
454 
455  // P only
456  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
457  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
458  }
459 #endif // USE_MAGNETOMETER_ONGROUND
460 #endif
461 
462  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
463  Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I));
464  if (Integrator_magnitude > RadOfDeg(300)) {
465  Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
466  }
467 
468 
469 }
470 /**************************************************/
471 
472 void Matrix_update(float dt)
473 {
474  Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
475  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
476 
477 #if OUTPUTMODE==1 // With corrected data (drift correction)
478  Update_Matrix[0][0] = 0;
479  Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
480  Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
481  Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
482  Update_Matrix[1][1] = 0;
483  Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
484  Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
485  Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
486  Update_Matrix[2][2] = 0;
487 #else // Uncorrected data (no drift correction)
488  Update_Matrix[0][0] = 0;
489  Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
490  Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
491  Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
492  Update_Matrix[1][1] = 0;
493  Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
494  Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
495  Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
496  Update_Matrix[2][2] = 0;
497 #endif
498 
500 
501  for (int x = 0; x < 3; x++) { //Matrix Addition (update)
502  for (int y = 0; y < 3; y++) {
503  DCM_Matrix[x][y] += Temporary_Matrix[x][y];
504  }
505  }
506 }
507 
509 {
510 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
511  ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
512  ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
514 #else
515  ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
516  ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
517  ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
518  ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
519 #endif
520 }
521 
523 {
525 }
526 
528 {
530 
531  if (!ahrs_dcm.is_aligned) {
532  /* Set ltp_to_imu so that body is zero */
534  }
535 }
angular rates
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
bool_t launch
Definition: sim_ap.c:39
float phi
in radians
#define float_rmat_of_eulers
static void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
void Matrix_update(float dt)
struct FloatVect3 accel_float
#define FLOAT_EULERS_ZERO(_e)
#define Kp_YAW
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
float r
in rad/s
void ahrs_dcm_init(void)
struct FloatVect3 mag_float
Utility functions for floating point AHRS implementations.
#define Ki_ROLLPITCH
#define FLOAT_RATES_ZERO(_r)
#define Ki_YAW
#define MAG_FLOAT_OF_BFP(_ai)
float psi
in radians
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
float DCM_Matrix[3][3]
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:75
void Drift_correction(void)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
float q
in rad/s
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:371
float p
in rad/s
float Update_Matrix[3][3]
#define FALSE
Definition: std.h:5
Roation quaternion.
float imu_health
int renorm_sqrt_count
struct FloatEulers ltp_to_imu_euler
float theta
in radians
void Normalize(void)
float Omega_Vector[3]
#define TRUE
Definition: std.h:4
static float Vector_Dot_Product(float vector1[3], float vector2[3])
float Omega[3]
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:692
Paparazzi floating point algebra.
data structure for GPS information
Definition: gps.h:67
struct AhrsFloatDCM ahrs_dcm
Device independent GPS code (interface)
Algebra helper functions for DCM.
int renorm_blowup_count
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct FloatRates gyro_bias
void ahrs_dcm_update_gps(struct GpsState *gps_s)
void ahrs_dcm_update_mag(struct Int32Vect3 *mag)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:728
enum AhrsDCMStatus status
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
bool_t is_aligned
float MAG_Heading_Y
struct OrientationReps body_to_imu
unsigned char uint8_t
Definition: types.h:14
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:76
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:593
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
float Temporary_Matrix[3][3]
uint8_t gps_age
void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
rotation matrix
bool_t gps_course_valid
arch independent LED (Light Emitting Diodes) API
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:74
float Omega_I[3]
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:77
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
Attitude estimation for fixedwings based on the DCM.
#define GRAVITY
static void compute_ahrs_representations(void)
void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
void ahrs_dcm_update_accel(struct Int32Vect3 *accel)
float MAG_Heading_X
uint8_t fix
status of fix
Definition: gps.h:82
float gps_acceleration
float Omega_P[3]
angular rates
#define Kp_ROLLPITCH
struct FloatRates imu_rate
void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Fixedwing autopilot modes.