Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
30 #include "subsystems/ahrs.h"
34 #include "subsystems/imu.h"
35 #include "firmwares/fixedwing/autopilot.h" // launch detection
36 
39 
40 #include "state.h"
41 
42 #if USE_GPS
43 #include "subsystems/gps.h"
44 #endif
45 
46 #include <string.h>
47 
48 #include "led.h"
49 
50 #if FLOAT_DCM_SEND_DEBUG
51 // FIXME Debugging Only
52 #include "mcu_periph/uart.h"
53 #include "messages.h"
55 #endif
56 
57 #ifndef AHRS_PROPAGATE_FREQUENCY
58 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
59 #endif
60 
61 // FIXME this is still needed for fixedwing integration
62 // remotely settable
63 #ifndef INS_ROLL_NEUTRAL_DEFAULT
64 #define INS_ROLL_NEUTRAL_DEFAULT 0
65 #endif
66 #ifndef INS_PITCH_NEUTRAL_DEFAULT
67 #define INS_PITCH_NEUTRAL_DEFAULT 0
68 #endif
71 
72 
74 
75 // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
76 // Positive pitch : nose up
77 // Positive roll : right wing down
78 // Positive yaw : clockwise
79 
80 // DCM Working variables
81 const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY );
82 
83 struct FloatVect3 accel_float = {0,0,0};
84 
85 float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
86 float Omega_P[3]= {0,0,0}; //Omega Proportional correction
87 float Omega_I[3]= {0,0,0}; //Omega Integrator
88 float Omega[3]= {0,0,0};
89 
90 float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
91 float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
92 float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
93 
94 #if USE_MAGNETOMETER
95 float MAG_Heading_X = 1;
96 float MAG_Heading_Y = 0;
97 #endif
98 
99 static inline void compute_ahrs_representations(void);
100 static inline void set_body_orientation_and_rates(void);
101 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
102 
103 void Normalize(void);
104 void Drift_correction(void);
105 void Matrix_update(void);
106 
107 #if PERFORMANCE_REPORTING == 1
108 int renorm_sqrt_count = 0;
109 int renorm_blowup_count = 0;
110 float imu_health = 0.;
111 #endif
112 
113 
114 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
115 {
116  for (int i=0; i<3; i++) {
117  for (int j=0; j<3; j++) {
118  DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
119  }
120  }
121 }
122 
123 
124 void ahrs_init(void) {
126 
127  /* Set ltp_to_imu so that body is zero */
129  sizeof(struct FloatEulers));
130 
132 
133  /* set inital filter dcm */
135 
136  ahrs_impl.gps_speed = 0;
138  ahrs_impl.gps_course = 0;
140  ahrs_impl.gps_age = 100;
141 }
142 
143 void ahrs_align(void)
144 {
145  /* Compute an initial orientation using euler angles */
147 
148  /* Convert initial orientation in quaternion and rotation matrice representations. */
149  struct FloatRMat ltp_to_imu_rmat;
151 
152  /* set filter dcm */
153  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
154 
155  /* Set initial body orientation */
157 
158  /* use averaged gyro as initial value for bias */
159  struct Int32Rates bias0;
162 
164 }
165 
166 
167 void ahrs_propagate(void)
168 {
169  /* convert imu data to floating point */
170  struct FloatRates gyro_float;
171  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
172 
173  /* unbias rate measurement */
175 
176  /* Uncouple Motions */
177 #ifdef IMU_GYRO_P_Q
178  float dp=0,dq=0,dr=0;
179  dp += ahrs_impl.imu_rate.q * IMU_GYRO_P_Q;
180  dp += ahrs_impl.imu_rate.r * IMU_GYRO_P_R;
181  dq += ahrs_impl.imu_rate.p * IMU_GYRO_Q_P;
182  dq += ahrs_impl.imu_rate.r * IMU_GYRO_Q_R;
183  dr += ahrs_impl.imu_rate.p * IMU_GYRO_R_P;
184  dr += ahrs_impl.imu_rate.q * IMU_GYRO_R_Q;
185 
186  ahrs_impl.imu_rate.p += dp;
187  ahrs_impl.imu_rate.q += dq;
188  ahrs_impl.imu_rate.r += dr;
189 #endif
190 
191  Matrix_update();
192 
193  Normalize();
194 
196 }
197 
198 void ahrs_update_gps(void)
199 {
200  static float last_gps_speed_3d = 0;
201 
202 #if USE_GPS
203  if (gps.fix == GPS_FIX_3D) {
204  ahrs_impl.gps_age = 0;
206 
207  if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s
208  ahrs_impl.gps_course = ((float)gps.course)/1.e7;
210  } else {
212  }
213  } else {
214  ahrs_impl.gps_age = 100;
215  }
216 #endif
217 
218  ahrs_impl.gps_acceleration += ( ((ahrs_impl.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_impl.gps_acceleration) / 5.0f;
219  last_gps_speed_3d = ahrs_impl.gps_speed;
220 }
221 
222 
224 {
225  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
226 
227  // DCM filter uses g-force as positive
228  // accelerometer measures [0 0 -g] in a static case
229  accel_float.x = -accel_float.x;
230  accel_float.y = -accel_float.y;
231  accel_float.z = -accel_float.z;
232 
233 
234  ahrs_impl.gps_age ++;
235  if (ahrs_impl.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
236 #if USE_AHRS_GPS_ACCELERATIONS
237 PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
238  accel_float.x += ahrs_impl.gps_acceleration; // Longitudinal acceleration
239 #endif
240  accel_float.y += ahrs_impl.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
241  accel_float.z -= ahrs_impl.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
242  }
243  else
244  {
245  ahrs_impl.gps_speed = 0;
247  ahrs_impl.gps_age = 100;
248  }
249 
251 }
252 
253 
254 void ahrs_update_mag(void)
255 {
256 #if USE_MAGNETOMETER
257 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
258 
259  float cos_roll;
260  float sin_roll;
261  float cos_pitch;
262  float sin_pitch;
263 
264  cos_roll = cosf(ahrs_impl.ltp_to_imu_euler.phi);
265  sin_roll = sinf(ahrs_impl.ltp_to_imu_euler.phi);
266  cos_pitch = cosf(ahrs_impl.ltp_to_imu_euler.theta);
267  sin_pitch = sinf(ahrs_impl.ltp_to_imu_euler.theta);
268 
269 
270  // Pitch&Roll Compensation:
271  MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
272  MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;
273 
274 /*
275  *
276  // Magnetic Heading
277  Heading = atan2(-Head_Y,Head_X);
278 
279  // Declination correction (if supplied)
280  if( declination != 0.0 )
281  {
282  Heading = Heading + declination;
283  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
284  Heading -= (2.0 * M_PI);
285  else if (Heading < -M_PI)
286  Heading += (2.0 * M_PI);
287  }
288 
289  // Optimization for external DCM use. Calculate normalized components
290  Heading_X = cos(Heading);
291  Heading_Y = sin(Heading);
292 */
293 
294  struct FloatVect3 ltp_mag;
295 
296  ltp_mag.x = MAG_Heading_X;
297  ltp_mag.y = MAG_Heading_Y;
298 
299 #if FLOAT_DCM_SEND_DEBUG
300  // Downlink
301  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
302 #endif
303 
304  // Magnetic Heading
305  // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
306 #endif
307 }
308 
309 void Normalize(void)
310 {
311  float error=0;
312  float temporary[3][3];
313  float renorm=0;
314  uint8_t problem=FALSE;
315 
316  // Find the non-orthogonality of X wrt Y
317  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
318 
319  // Add half the XY error to X, and half to Y
320  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
321  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
322  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
323  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
324 
325  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
326  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
327 
328  // Normalize lenght of X
329  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
330  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
331  // b) if the norm is further from 1, use a real sqrt
332  // c) norm is huge: disaster! reset! mayday!
333  if (renorm < 1.5625f && renorm > 0.64f) {
334  renorm= .5 * (3-renorm); //eq.21
335  } else if (renorm < 100.0f && renorm > 0.01f) {
336  renorm= 1. / sqrt(renorm);
337 #if PERFORMANCE_REPORTING == 1
339 #endif
340  } else {
341  problem = TRUE;
342 #if PERFORMANCE_REPORTING == 1
344 #endif
345  }
346  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
347 
348  // Normalize lenght of Y
349  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
350  if (renorm < 1.5625f && renorm > 0.64f) {
351  renorm= .5 * (3-renorm); //eq.21
352  } else if (renorm < 100.0f && renorm > 0.01f) {
353  renorm= 1. / sqrt(renorm);
354 #if PERFORMANCE_REPORTING == 1
356 #endif
357  } else {
358  problem = TRUE;
359 #if PERFORMANCE_REPORTING == 1
361 #endif
362  }
363  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
364 
365  // Normalize lenght of Z
366  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
367  if (renorm < 1.5625f && renorm > 0.64f) {
368  renorm= .5 * (3-renorm); //eq.21
369  } else if (renorm < 100.0f && renorm > 0.01f) {
370  renorm= 1. / sqrt(renorm);
371 #if PERFORMANCE_REPORTING == 1
373 #endif
374  } else {
375  problem = TRUE;
376 #if PERFORMANCE_REPORTING == 1
378 #endif
379  }
380  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
381 
382  // Reset on trouble
383  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
385  problem = FALSE;
386  }
387 }
388 
389 
391 {
392  //Compensation the Roll, Pitch and Yaw drift.
393  static float Scaled_Omega_P[3];
394  static float Scaled_Omega_I[3];
395  float Accel_magnitude;
396  float Accel_weight;
397  float Integrator_magnitude;
398 
399  // Local Working Variables
400  float errorRollPitch[3];
401  float errorYaw[3];
402  float errorCourse;
403 
404  //*****Roll and Pitch***************
405 
406  // Calculate the magnitude of the accelerometer vector
407  Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
408  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
409  // Dynamic weighting of accelerometer info (reliability filter)
410  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
411  Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
412 
413 
414  #if PERFORMANCE_REPORTING == 1
415  {
416 
417  float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
418  imu_health += tempfloat;
419  Bound(imu_health,129,65405);
420  }
421  #endif
422 
423  Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
424  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
425 
426  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
427  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
428 
429  //*****YAW***************
430 
431 #if USE_MAGNETOMETER
432  // We make the gyro YAW drift correction based on compass magnetic heading
433 // float mag_heading_x = cos(MAG_Heading);
434 // float mag_heading_y = sin(MAG_Heading);
435  // 2D dot product
436  errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
437  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
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_impl.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  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
454 
455  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
456  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
457 
458  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
459  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
460  }
461 #if USE_MAGNETOMETER_ONGROUND == 1
462 PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
463  else if (launch == FALSE)
464  {
465  float COGX = imu.mag.x; // Non-Tilt-Compensated (for filter stability reasons)
466  float COGY = imu.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  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //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
479  Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
480  if (Integrator_magnitude > RadOfDeg(300)) {
481  Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude);
482  }
483 
484 
485 }
486 /**************************************************/
487 
488 void Matrix_update(void)
489 {
490  Vector_Add(&Omega[0], &ahrs_impl.imu_rate.p, &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]=-G_Dt*Omega_Vector[2];//-z
496  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
497  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
498  Update_Matrix[1][1]=0;
499  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
500  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
501  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
502  Update_Matrix[2][2]=0;
503  #else // Uncorrected data (no drift correction)
504  Update_Matrix[0][0]=0;
508  Update_Matrix[1][1]=0;
512  Update_Matrix[2][2]=0;
513  #endif
514 
516 
517  for(int x=0; x<3; x++) //Matrix Addition (update)
518  {
519  for(int y=0; y<3; y++)
520  {
522  }
523  }
524 }
525 
526 /*
527  * Compute body orientation and rates from imu orientation and rates
528  */
529 static inline void set_body_orientation_and_rates(void) {
530 
531  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
532 
533  struct FloatRates body_rate;
534  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate);
535  stateSetBodyRates_f(&body_rate);
536 
537  struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
539  FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat);
540 
541  // Some stupid lines of code for neutrals
542  struct FloatEulers ltp_to_body_euler;
543  FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
544  ltp_to_body_euler.phi -= ins_roll_neutral;
545  ltp_to_body_euler.theta -= ins_pitch_neutral;
546  stateSetNedToBodyEulers_f(&ltp_to_body_euler);
547 
548  // should be replaced at the end by:
549  // stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
550 
551 }
552 
553 static inline void compute_ahrs_representations(void) {
554 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
555  ahrs_impl.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
556  ahrs_impl.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
558 #else
559  ahrs_impl.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
561  ahrs_impl.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
562  ahrs_impl.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
563 #endif
564 
566 
567  /*
568  RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
569  &(DCM_Matrix[0][0]),
570  &(DCM_Matrix[0][1]),
571  &(DCM_Matrix[0][2]),
572 
573  &(DCM_Matrix[1][0]),
574  &(DCM_Matrix[1][1]),
575  &(DCM_Matrix[1][2]),
576 
577  &(DCM_Matrix[2][0]),
578  &(DCM_Matrix[2][1]),
579  &(DCM_Matrix[2][2])
580 
581  ));
582  */
583 }
584 
#define FLOAT_RATES_ZERO(_r)
Interface to align the AHRS via low-passed measurements at startup.
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
rotation matrix
bool_t launch
Definition: sim_ap.c:40
Attitude and Heading Reference System interface.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
static void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
struct FloatVect3 accel_float
void Matrix_update(void)
angular rates
float psi
in radians
#define Kp_YAW
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
Utility functions for floating point AHRS implementations.
#define Ki_ROLLPITCH
#define INS_ROLL_NEUTRAL_DEFAULT
#define Ki_YAW
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
void ahrs_align(void)
Aligns the AHRS.
float DCM_Matrix[3][3]
float theta
in radians
uint8_t fix
status of fix
Definition: gps.h:78
void Drift_correction(void)
#define GPS_FIX_3D
Definition: gps.h:43
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
euler angles
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
float Update_Matrix[3][3]
#define FALSE
Definition: imu_chimu.h:141
void ahrs_propagate(void)
Propagation.
float gps_acceleration
float imu_health
int renorm_sqrt_count
struct AhrsFloatDCM ahrs_impl
float p
in rad/s
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
void Normalize(void)
float Omega_Vector[3]
static float Vector_Dot_Product(float vector1[3], float vector2[3])
static void set_body_orientation_and_rates(void)
float Omega[3]
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
Paparazzi floating point algebra.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float phi
in radians
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
Device independent GPS code (interface)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
Algebra helper functions for DCM.
#define INS_PITCH_NEUTRAL_DEFAULT
int renorm_blowup_count
float ins_pitch_neutral
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573
Inertial Measurement Unit interface.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:672
float r
in rad/s
uint8_t gps_age
#define AHRS_PROPAGATE_FREQUENCY
struct FloatRates gyro_bias
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
float MAG_Heading_Y
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
unsigned char uint8_t
Definition: types.h:14
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
API to get/set the generic vehicle states.
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
float Temporary_Matrix[3][3]
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
void ahrs_init(void)
AHRS initialization.
arch independent LED (Light Emitting Diodes) API
float ins_roll_neutral
float Omega_I[3]
float q
in rad/s
Attitude estimation for fixedwings based on the DCM.
struct FloatEulers ltp_to_imu_euler
#define GRAVITY
const float G_Dt
static void compute_ahrs_representations(void)
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
float MAG_Heading_X
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
bool_t gps_course_valid
struct GpsState gps
global GPS state
Definition: gps.c:41
float Omega_P[3]
#define Kp_ROLLPITCH
#define AHRS_RUNNING
Definition: ahrs.h:36
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
struct FloatRates imu_rate
Fixedwing autopilot modes.