Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
21 #include "std.h"
22 
23 #include "subsystems/ahrs.h"
27 #include "subsystems/imu.h"
28 
31 
32 #if USE_GPS
33 #include "subsystems/gps.h"
34 #endif
35 
36 #include <string.h>
37 
38 #include "led.h"
39 
40 // FIXME Debugging Only
41 #ifndef DOWNLINK_DEVICE
42 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
43 #endif
44 #include "mcu_periph/uart.h"
45 #include "messages.h"
47 
48 
49 #ifdef AHRS_UPDATE_FW_ESTIMATOR
50 // FIXME this is still needed for fixedwing integration
51 #include "estimator.h"
52 // remotely settable
53 #ifndef INS_ROLL_NEUTRAL_DEFAULT
54 #define INS_ROLL_NEUTRAL_DEFAULT 0
55 #endif
56 #ifndef INS_PITCH_NEUTRAL_DEFAULT
57 #define INS_PITCH_NEUTRAL_DEFAULT 0
58 #endif
59 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
60 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
61 #endif /* AHRS_UPDATE_FW_ESTIMATOR */
62 
63 
65 
66 // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
67 // Positive pitch : nose up
68 // Positive roll : right wing down
69 // Positive yaw : clockwise
70 
71 // DCM Working variables
72 const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY );
73 
74 struct FloatVect3 accel_float = {0,0,0};
75 
76 float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
77 float Omega_P[3]= {0,0,0}; //Omega Proportional correction
78 float Omega_I[3]= {0,0,0}; //Omega Integrator
79 float Omega[3]= {0,0,0};
80 
81 float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
82 float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
83 float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
84 
85 #if USE_MAGNETOMETER
86 float MAG_Heading_X = 1;
87 float MAG_Heading_Y = 0;
88 #endif
89 
90 static inline void compute_ahrs_representations(void);
91 static inline void compute_body_orientation_and_rates(void);
92 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
93 
94 void Normalize(void);
95 void Drift_correction(void);
96 void Matrix_update(void);
97 
98 #if PERFORMANCE_REPORTING == 1
99 int renorm_sqrt_count = 0;
100 int renorm_blowup_count = 0;
101 float imu_health = 0.;
102 #endif
103 
104 #if USE_HIGH_ACCEL_FLAG
105 // High Accel Flag
106 #define HIGH_ACCEL_LOW_SPEED 15.0
107 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
108 #define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
109 #define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
110 bool_t high_accel_done;
111 bool_t high_accel_flag;
112 // Command vector for thrust (fixed_wing)
113 #include "inter_mcu.h"
114 #endif
115 
116 
117 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
118 {
119  for (int i=0; i<3; i++) {
120  for (int j=0; j<3; j++) {
121  DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
122  }
123  }
124 }
125 
126 
127 void ahrs_init(void) {
129 
130  /*
131  * Initialises our IMU alignement variables
132  * This should probably done in the IMU code instead
133  */
134  struct FloatEulers body_to_imu_euler =
136  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
137  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
138 
139  /* set ltp_to_body to zero */
144 
145  /* set ltp_to_imu so that body is zero */
148  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
150 
151  /* set inital filter dcm */
153 
154 #if USE_HIGH_ACCEL_FLAG
157 #endif
158 }
159 
160 void ahrs_align(void)
161 {
162  /* Compute an initial orientation using euler angles */
164 
165  /* Convert initial orientation in quaternion and rotation matrice representations. */
168 
169  /* set filter dcm */
171 
172  /* Compute initial body orientation */
174 
175  /* use averaged gyro as initial value for bias */
176  struct Int32Rates bias0;
179 
181 }
182 
183 
184 void ahrs_propagate(void)
185 {
186  /* convert imu data to floating point */
187  struct FloatRates gyro_float;
188  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
189 
190  /* unbias rate measurement */
192 
193  /* Uncouple Motions */
194 #ifdef IMU_GYRO_P_Q
195  float dp=0,dq=0,dr=0;
196  dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q;
197  dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R;
198  dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P;
199  dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R;
200  dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P;
201  dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q;
202 
203  ahrs_float.imu_rate.p += dp;
204  ahrs_float.imu_rate.q += dq;
205  ahrs_float.imu_rate.r += dr;
206 #endif
207 
208  Matrix_update();
209 
210  Normalize();
211 
213 }
214 
216 {
217 
218  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
219 
220  // DCM filter uses g-force as positive
221  // accelerometer measures [0 0 -g] in a static case
222  accel_float.x = -accel_float.x;
223  accel_float.y = -accel_float.y;
224  accel_float.z = -accel_float.z;
225 
226 
227 #if USE_GPS
228  if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration.
229  accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
230  accel_float.z -= gps.speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
231  }
232 #endif
233 
235 }
236 
237 
238 void ahrs_update_mag(void)
239 {
240 #if USE_MAGNETOMETER
241 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
242 
243  float cos_roll;
244  float sin_roll;
245  float cos_pitch;
246  float sin_pitch;
247 
248  cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi);
249  sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi);
250  cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta);
251  sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta);
252 
253 
254  // Pitch&Roll Compensation:
255  MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
256  MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;
257 
258 /*
259  *
260  // Magnetic Heading
261  Heading = atan2(-Head_Y,Head_X);
262 
263  // Declination correction (if supplied)
264  if( declination != 0.0 )
265  {
266  Heading = Heading + declination;
267  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
268  Heading -= (2.0 * M_PI);
269  else if (Heading < -M_PI)
270  Heading += (2.0 * M_PI);
271  }
272 
273  // Optimization for external DCM use. Calculate normalized components
274  Heading_X = cos(Heading);
275  Heading_Y = sin(Heading);
276 */
277 
278  struct FloatVect3 ltp_mag;
279 
280  ltp_mag.x = MAG_Heading_X;
281  ltp_mag.y = MAG_Heading_Y;
282 
283  // Downlink
284  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
285 
286  // Magnetic Heading
287  // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
288 #endif
289 }
290 
291 void ahrs_update_gps(void) {
292 
293 }
294 
295 void Normalize(void)
296 {
297  float error=0;
298  float temporary[3][3];
299  float renorm=0;
300  uint8_t problem=FALSE;
301 
302  // Find the non-orthogonality of X wrt Y
303  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
304 
305  // Add half the XY error to X, and half to Y
306  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
307  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
308  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
309  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
310 
311  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
312  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
313 
314  // Normalize lenght of X
315  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
316  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
317  // b) if the norm is further from 1, use a real sqrt
318  // c) norm is huge: disaster! reset! mayday!
319  if (renorm < 1.5625f && renorm > 0.64f) {
320  renorm= .5 * (3-renorm); //eq.21
321  } else if (renorm < 100.0f && renorm > 0.01f) {
322  renorm= 1. / sqrt(renorm);
323 #if PERFORMANCE_REPORTING == 1
325 #endif
326  } else {
327  problem = TRUE;
328 #if PERFORMANCE_REPORTING == 1
330 #endif
331  }
332  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
333 
334  // Normalize lenght of Y
335  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
336  if (renorm < 1.5625f && renorm > 0.64f) {
337  renorm= .5 * (3-renorm); //eq.21
338  } else if (renorm < 100.0f && renorm > 0.01f) {
339  renorm= 1. / sqrt(renorm);
340 #if PERFORMANCE_REPORTING == 1
342 #endif
343  } else {
344  problem = TRUE;
345 #if PERFORMANCE_REPORTING == 1
347 #endif
348  }
349  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
350 
351  // Normalize lenght of Z
352  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
353  if (renorm < 1.5625f && renorm > 0.64f) {
354  renorm= .5 * (3-renorm); //eq.21
355  } else if (renorm < 100.0f && renorm > 0.01f) {
356  renorm= 1. / sqrt(renorm);
357 #if PERFORMANCE_REPORTING == 1
359 #endif
360  } else {
361  problem = TRUE;
362 #if PERFORMANCE_REPORTING == 1
364 #endif
365  }
366  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
367 
368  // Reset on trouble
369  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
371  problem = FALSE;
372  }
373 }
374 
375 
377 {
378  //Compensation the Roll, Pitch and Yaw drift.
379  static float Scaled_Omega_P[3];
380  static float Scaled_Omega_I[3];
381  float Accel_magnitude;
382  float Accel_weight;
383  float Integrator_magnitude;
384 
385  // Local Working Variables
386  float errorRollPitch[3];
387  float errorYaw[3];
388  float errorCourse;
389 
390  //*****Roll and Pitch***************
391 
392  // Calculate the magnitude of the accelerometer vector
393  Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
394  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
395  // Dynamic weighting of accelerometer info (reliability filter)
396  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
397  Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
398 
399 #if USE_HIGH_ACCEL_FLAG
400  // Test for high acceleration:
401  // - low speed
402  // - high thrust
403  if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
405  } else {
408  high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
409  }
410  if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
411  high_accel_done = FALSE; // Activate high accel after landing
412  }
413  }
414  if (high_accel_flag) { Accel_weight = 0.; }
415 #endif
416 
417 
418  #if PERFORMANCE_REPORTING == 1
419  {
420 
421  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
422  imu_health += tempfloat;
423  Bound(imu_health,129,65405);
424  }
425  #endif
426 
427  Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
428  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
429 
430  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
431  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
432 
433  //*****YAW***************
434 
435 #if USE_MAGNETOMETER
436  // We make the gyro YAW drift correction based on compass magnetic heading
437 // float mag_heading_x = cos(MAG_Heading);
438 // float mag_heading_y = sin(MAG_Heading);
439  // 2D dot product
440  errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
441  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
442 
443  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
444  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
445 
446  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
447  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
448 
449 #elif USE_GPS // Use GPS Ground course to correct yaw gyro drift
450 
451  if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
452  float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad
453  float COGX = cosf(ground_course); //Course overground X axis
454  float COGY = sinf(ground_course); //Course overground Y axis
455 
456  errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
457  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
458 
459  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
460  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
461 
462  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
463  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
464  }
465 #endif
466 
467  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
468  Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
469  if (Integrator_magnitude > RadOfDeg(300)) {
470  Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude);
471  }
472 
473 
474 }
475 /**************************************************/
476 
477 void Matrix_update(void)
478 {
479  Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term
480  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
481 
482  #if OUTPUTMODE==1 // With corrected data (drift correction)
483  Update_Matrix[0][0]=0;
484  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
485  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
486  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
487  Update_Matrix[1][1]=0;
488  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
489  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
490  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
491  Update_Matrix[2][2]=0;
492  #else // Uncorrected data (no drift correction)
493  Update_Matrix[0][0]=0;
497  Update_Matrix[1][1]=0;
501  Update_Matrix[2][2]=0;
502  #endif
503 
505 
506  for(int x=0; x<3; x++) //Matrix Addition (update)
507  {
508  for(int y=0; y<3; y++)
509  {
511  }
512  }
513 }
514 
515 /*
516  * Compute body orientation and rates from imu orientation and rates
517  */
518 static inline void compute_body_orientation_and_rates(void) {
519 
526 
527 }
528 
529 static inline void compute_ahrs_representations(void) {
530 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
531  ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
532  ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
534 #else
535  ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
537  ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
538  ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
539 #endif
540 
541  /* set quaternion and rotation matrix representations as well */
544 
546 
547  /*
548  RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
549  &(DCM_Matrix[0][0]),
550  &(DCM_Matrix[0][1]),
551  &(DCM_Matrix[0][2]),
552 
553  &(DCM_Matrix[1][0]),
554  &(DCM_Matrix[1][1]),
555  &(DCM_Matrix[1][2]),
556 
557  &(DCM_Matrix[2][0]),
558  &(DCM_Matrix[2][1]),
559  &(DCM_Matrix[2][2])
560 
561  ));
562  */
563 }
564 
565 #ifdef AHRS_UPDATE_FW_ESTIMATOR
566 void ahrs_update_fw_estimator( void ) {
567  // export results to estimator
571 
575 }
576 #endif //AHRS_UPDATE_FW_ESTIMATOR
#define FLOAT_RATES_ZERO(_r)
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:82
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:338
struct FloatRMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:68
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:59
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
rotation matrix
Attitude and Heading Reference System interface.
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
float estimator_r
Definition: estimator.c:56
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
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
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:61
void Matrix_update(void)
angular rates
#define FLOAT_RMAT_ZERO(_rm)
bool_t high_accel_done
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
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:36
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:303
void ahrs_update_gps(void)
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
#define Ki_ROLLPITCH
#define Ki_YAW
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define AHRS_PROPAGATE_FREQUENCY
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:77
void Drift_correction(void)
#define GPS_FIX_3D
Definition: gps.h:42
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
euler angles
#define FLOAT_QUAT_OF_EULERS(_q, _e)
float estimator_p
Definition: estimator.c:54
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:37
float Update_Matrix[3][3]
struct FloatQuat body_to_imu_quat
#define FALSE
Definition: imu_chimu.h:141
void ahrs_propagate(void)
Propagation.
float imu_health
int renorm_sqrt_count
struct AhrsFloatDCM ahrs_impl
float p
in rad/s^2
#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])
float Omega[3]
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:30
Paparazzi floating point algebra.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float phi
in radians
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:564
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
struct FloatRMat body_to_imu_rmat
int renorm_blowup_count
#define HIGH_ACCEL_HIGH_THRUST_RESUME
#define HIGH_ACCEL_HIGH_THRUST
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:520
Inertial Measurement Unit interface.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:619
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:62
float ins_pitch_neutral
Definition: ins_arduimu.c:15
float r
in rad/s^2
struct FloatRates gyro_bias
#define AHRS_UNINIT
Definition: ahrs.h:33
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
void ahrs_update_fw_estimator(void)
#define FLOAT_EULERS_ZERO(_e)
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Definition: commands.c:42
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:478
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
bool_t high_accel_flag
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
float Temporary_Matrix[3][3]
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:631
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
State estimation, fusioning sensors.
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
void ahrs_init(void)
AHRS initialization.
arch independent LED (Light Emitting Diodes) API
float Omega_I[3]
float q
in rad/s^2
Attitude estimation for fixedwings based on the DCM Theory: http://code.google.com/p/gentlenav/downlo...
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float ins_roll_neutral
Definition: ins_arduimu.c:14
#define GRAVITY
const float G_Dt
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69
static void compute_ahrs_representations(void)
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:83
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#define FLOAT_QUAT_ZERO(_q)
#define HIGH_ACCEL_LOW_SPEED
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:81
struct GpsState gps
global GPS state
Definition: gps.c:31
float Omega_P[3]
#define Kp_ROLLPITCH
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:236
#define AHRS_RUNNING
Definition: ahrs.h:34
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:35
struct FloatEulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:60
static void compute_body_orientation_and_rates(void)