Paparazzi UAS  v5.17_devel-114-gb0d9eaf
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 "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 "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 
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_imu euler with zero */
106 
108 
109  /* set inital filter dcm */
111 
112  ahrs_dcm.gps_speed = 0;
114  ahrs_dcm.gps_course = 0;
115  ahrs_dcm.gps_course_valid = false;
116  ahrs_dcm.gps_age = 100;
117 }
118 
119 bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
120  struct FloatVect3 *lp_mag __attribute__((unused)))
121 {
122  /* Compute an initial orientation in quaternion (and then back to euler angles) so it works upside-down as well */
123  struct FloatQuat quat;
124 #if USE_MAGNETOMETER
125  ahrs_float_get_quat_from_accel_mag(&quat, lp_accel, lp_mag);
126 #else
127  ahrs_float_get_quat_from_accel(&quat, lp_accel);
128 #endif
130 
131  /* Convert initial orientation from quaternion to rotation matrix representations. */
132  struct FloatRMat ltp_to_imu_rmat;
133  float_rmat_of_quat(&ltp_to_imu_rmat, &quat);
134 
135  /* set filter dcm */
136  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
137 
138  /* use averaged gyro as initial value for bias */
139  ahrs_dcm.gyro_bias = *lp_gyro;
140 
142  ahrs_dcm.is_aligned = true;
143 
144  return true;
145 }
146 
147 
148 void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
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 //FIXME: Should be settable
185  ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
186  ahrs_dcm.gps_course_valid = true;
187  } else {
188  ahrs_dcm.gps_course_valid = false;
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  // DCM filter uses g-force as positive
203  // accelerometer measures [0 0 -g] in a static case
204  accel_float.x = -accel->x;
205  accel_float.y = -accel->y;
206  accel_float.z = -accel->z;
207 
208 
209  ahrs_dcm.gps_age ++;
210  if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
211 #if USE_AHRS_GPS_ACCELERATIONS
212  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
213  accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
214 #endif
215  accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
216  accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
217  } else {
218  ahrs_dcm.gps_speed = 0;
220  ahrs_dcm.gps_age = 100;
221  }
222 
224 }
225 
226 
228 {
229 #if USE_MAGNETOMETER
230 MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
231 
232  float cos_roll;
233  float sin_roll;
234  float cos_pitch;
235  float sin_pitch;
236 
237  cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
238  sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
239  cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
240  sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
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 
270  ltp_mag.x = MAG_Heading_X;
271  ltp_mag.y = MAG_Heading_Y;
272 
273  // Downlink
274  RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
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 
286 void 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
306  renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
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!
310  if (renorm < 1.5625f && renorm > 0.64f) {
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
326  renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
327  if (renorm < 1.5625f && renorm > 0.64f) {
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
343  renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
344  if (renorm < 1.5625f && renorm > 0.64f) {
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!
362  problem = false;
363  }
364 }
365 
366 // strong structural vibrations can prevent to perform the drift correction
367 // so accel magnitude is filtered before computing the weighting heuristic
368 #ifndef ACCEL_WEIGHT_FILTER
369 #define ACCEL_WEIGHT_FILTER 8
370 #endif
371 
372 // the weigthing is function of the length of a band of 1G by default
373 // so <0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0
374 // adjust the band size if needed, the value should be >0
375 #ifndef ACCEL_WEIGHT_BAND
376 #define ACCEL_WEIGHT_BAND 1.f
377 #endif
378 
380 {
381  //Compensation the Roll, Pitch and Yaw drift.
382  static float Scaled_Omega_P[3];
383  static float Scaled_Omega_I[3];
384  static float Accel_filtered = 0.f;
385  float Accel_magnitude;
386  float Accel_weight;
387  float Integrator_magnitude;
388 
389  // Local Working Variables
390  float errorRollPitch[3];
391  float errorYaw[3];
392  float errorCourse;
393 
394  //*****Roll and Pitch***************
395 
396  // Calculate the magnitude of the accelerometer vector
397  Accel_magnitude = sqrtf(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
398  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
399 #if ACCEL_WEIGHT_FILTER
400  Accel_filtered = (Accel_magnitude + (ACCEL_WEIGHT_FILTER - 1) * Accel_filtered) / ACCEL_WEIGHT_FILTER;
401 #else // set ACCEL_WEIGHT_FILTER to 0 to disable filter
402  Accel_filtered = Accel_magnitude;
403 #endif
404  // Dynamic weighting of accelerometer info (reliability filter)
405  // Weight for accelerometer info according to band size (min value is 0.1 to prevent division by zero)
406  Accel_weight = Clip(1.f - (2.f / Max(0.1f,ACCEL_WEIGHT_BAND)) * fabsf(1.f - Accel_filtered), 0.f, 1.f);
407 
408 
409 #if PERFORMANCE_REPORTING == 1
410  {
411  //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
412  float tempfloat = ((Accel_weight - 0.5) * 256.0f);
413  imu_health += tempfloat;
414  Bound(imu_health, 129, 65405);
415  }
416 #endif
417 
418  Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
419  Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
420 
421  Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
422  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
423 
424  //*****YAW***************
425 
426 #if USE_MAGNETOMETER
427  // We make the gyro YAW drift correction based on compass magnetic heading
428 // float mag_heading_x = cos(MAG_Heading);
429 // float mag_heading_y = sin(MAG_Heading);
430  // 2D dot product
431  //Calculating YAW error
432  errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
433  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
434  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
435 
436  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
437  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
438 
439  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
440  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
441 
442 #else // Use GPS Ground course to correct yaw gyro drift
443 
445  float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
446  float COGX = cosf(course); //Course overground X axis
447  float COGY = sinf(course); //Course overground Y axis
448 
449  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
450  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
451  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
452 
453  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
454  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
455 
456  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
457  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
458  }
459 #if USE_MAGNETOMETER_ONGROUND
460  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
461  else if (autopilot.launch == FALSE) {
462  float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
463  float COGY = mag->y; // Non-Tilt-Compensated (for filter stability reasons)
464 
465  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
466  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
467  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
468 
469  // P only
470  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
471  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
472  }
473 #endif // USE_MAGNETOMETER_ONGROUND
474 #endif
475 
476  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
477  Integrator_magnitude = sqrtf(Vector_Dot_Product(Omega_I, Omega_I));
478  if (Integrator_magnitude > RadOfDeg(300)) {
479  Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
480  }
481 
482 
483 }
484 /**************************************************/
485 
486 void Matrix_update(float dt)
487 {
488  Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
489  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
490 
491 #if OUTPUTMODE==1 // With corrected data (drift correction)
492  Update_Matrix[0][0] = 0;
493  Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
494  Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
495  Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
496  Update_Matrix[1][1] = 0;
497  Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
498  Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
499  Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
500  Update_Matrix[2][2] = 0;
501 #else // Uncorrected data (no drift correction)
502  Update_Matrix[0][0] = 0;
503  Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
504  Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
505  Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
506  Update_Matrix[1][1] = 0;
507  Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
508  Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
509  Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
510  Update_Matrix[2][2] = 0;
511 #endif
512 
514 
515  for (int x = 0; x < 3; x++) { //Matrix Addition (update)
516  for (int y = 0; y < 3; y++) {
517  DCM_Matrix[x][y] += Temporary_Matrix[x][y];
518  }
519  }
520 }
521 
523 {
524 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
525  ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
526  ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
528 #else
529  ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
530  ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
531  ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
532  ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
533 #endif
534 }
535 
537 {
539 }
540 
542 {
544 
545  if (!ahrs_dcm.is_aligned) {
546  /* Set ltp_to_imu so that body is zero */
548  }
549 }
bool launch
request launch
Definition: autopilot.h:71
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
float phi
in radians
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
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).
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
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
float psi
in radians
#define ACCEL_WEIGHT_BAND
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:98
void Drift_correction(void)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float q
in rad/s
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
float p
in rad/s
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.
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
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]
static float Vector_Dot_Product(float vector1[3], float vector2[3])
float Omega[3]
Paparazzi floating point algebra.
data structure for GPS information
Definition: gps.h:87
struct AhrsFloatDCM ahrs_dcm
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
bool gps_course_valid
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_accel(struct FloatVect3 *accel)
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
#define ACCEL_WEIGHT_FILTER
#define Max(x, y)
Definition: main_fbw.c:53
enum AhrsDCMStatus status
Core autopilot interface common to all firmwares.
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
float MAG_Heading_Y
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
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:99
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
static int16_t course[3]
Definition: airspeed_uADC.c:57
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
arch independent LED (Light Emitting Diodes) API
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
float Omega_I[3]
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
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)
float MAG_Heading_X
uint8_t fix
status of fix
Definition: gps.h:107
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
float gps_acceleration
float Omega_P[3]
angular rates
#define Kp_ROLLPITCH
struct FloatRates imu_rate
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).