Paparazzi UAS  v4.2.2_stable-4-gcc32f65
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 
26 #include "std.h"
27 
28 #include "subsystems/ahrs.h"
32 #include "subsystems/imu.h"
33 #include "firmwares/fixedwing/autopilot.h" // launch detection
34 
37 
38 #if USE_GPS
39 #include "subsystems/gps.h"
40 #endif
41 
42 #include <string.h>
43 
44 #include "led.h"
45 
46 #if FLOAT_DCM_SEND_DEBUG
47 // FIXME Debugging Only
48 #ifndef DOWNLINK_DEVICE
49 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
50 #endif
51 #include "mcu_periph/uart.h"
52 #include "messages.h"
54 #endif
55 
56 
57 #ifdef AHRS_UPDATE_FW_ESTIMATOR
58 // FIXME this is still needed for fixedwing integration
59 #include "estimator.h"
60 // remotely settable
61 #ifndef INS_ROLL_NEUTRAL_DEFAULT
62 #define INS_ROLL_NEUTRAL_DEFAULT 0
63 #endif
64 #ifndef INS_PITCH_NEUTRAL_DEFAULT
65 #define INS_PITCH_NEUTRAL_DEFAULT 0
66 #endif
67 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
68 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
69 #endif /* AHRS_UPDATE_FW_ESTIMATOR */
70 
71 
73 
74 // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
75 // Positive pitch : nose up
76 // Positive roll : right wing down
77 // Positive yaw : clockwise
78 
79 // DCM Working variables
80 const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY );
81 
82 struct FloatVect3 accel_float = {0,0,0};
83 
84 float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
85 float Omega_P[3]= {0,0,0}; //Omega Proportional correction
86 float Omega_I[3]= {0,0,0}; //Omega Integrator
87 float Omega[3]= {0,0,0};
88 
89 float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
90 float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
91 float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
92 
93 #if USE_MAGNETOMETER
94 float MAG_Heading_X = 1;
95 float MAG_Heading_Y = 0;
96 #endif
97 
98 static inline void compute_ahrs_representations(void);
99 static inline void compute_body_orientation_and_rates(void);
100 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
101 
102 void Normalize(void);
103 void Drift_correction(void);
104 void Matrix_update(void);
105 
106 #if PERFORMANCE_REPORTING == 1
107 int renorm_sqrt_count = 0;
108 int renorm_blowup_count = 0;
109 float imu_health = 0.;
110 #endif
111 
112 #if USE_HIGH_ACCEL_FLAG
113 // High Accel Flag
114 #define HIGH_ACCEL_LOW_SPEED 15.0
115 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
116 #define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
117 #define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
118 bool_t high_accel_done;
119 bool_t high_accel_flag;
120 // Command vector for thrust (fixed_wing)
121 #include "inter_mcu.h"
122 #endif
123 
124 
125 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
126 {
127  for (int i=0; i<3; i++) {
128  for (int j=0; j<3; j++) {
129  DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
130  }
131  }
132 }
133 
134 
135 void ahrs_init(void) {
137 
138  /*
139  * Initialises our IMU alignement variables
140  * This should probably done in the IMU code instead
141  */
142  struct FloatEulers body_to_imu_euler =
144  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
145  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
146 
147  /* set ltp_to_body to zero */
152 
153  /* set ltp_to_imu so that body is zero */
156  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
158 
159  /* set inital filter dcm */
161 
162 #if USE_HIGH_ACCEL_FLAG
165 #endif
166 
167  ahrs_impl.gps_speed = 0;
169  ahrs_impl.gps_course = 0;
171  ahrs_impl.gps_age = 100;
172 }
173 
174 void ahrs_align(void)
175 {
176  /* Compute an initial orientation using euler angles */
178 
179  /* Convert initial orientation in quaternion and rotation matrice representations. */
182 
183  /* set filter dcm */
185 
186  /* Compute initial body orientation */
188 
189  /* use averaged gyro as initial value for bias */
190  struct Int32Rates bias0;
193 
195 }
196 
197 
198 void ahrs_propagate(void)
199 {
200  /* convert imu data to floating point */
201  struct FloatRates gyro_float;
202  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
203 
204  /* unbias rate measurement */
206 
207  /* Uncouple Motions */
208 #ifdef IMU_GYRO_P_Q
209  float dp=0,dq=0,dr=0;
210  dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q;
211  dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R;
212  dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P;
213  dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R;
214  dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P;
215  dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q;
216 
217  ahrs_float.imu_rate.p += dp;
218  ahrs_float.imu_rate.q += dq;
219  ahrs_float.imu_rate.r += dr;
220 #endif
221 
222  Matrix_update();
223 
224  Normalize();
225 
227 }
228 
229 void ahrs_update_gps(void)
230 {
231  static float last_gps_speed_3d = 0;
232 
233 #if USE_GPS
234  if (gps.fix == GPS_FIX_3D) {
235  ahrs_impl.gps_age = 0;
237 
238  if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
239  ahrs_impl.gps_course = ((float)gps.course)/1.e7;
241  } else {
243  }
244  } else {
245  ahrs_impl.gps_age = 100;
246  }
247 #endif
248 
249  ahrs_impl.gps_acceleration += ( ((ahrs_impl.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_impl.gps_acceleration) / 5.0f;
250  last_gps_speed_3d = ahrs_impl.gps_speed;
251 }
252 
253 
255 {
256  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
257 
258  // DCM filter uses g-force as positive
259  // accelerometer measures [0 0 -g] in a static case
260  accel_float.x = -accel_float.x;
261  accel_float.y = -accel_float.y;
262  accel_float.z = -accel_float.z;
263 
264 
265  ahrs_impl.gps_age ++;
266  if (ahrs_impl.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
267 #if USE_AHRS_GPS_ACCELERATIONS
268 #pragma message "AHRS_FLOAT_DCM uses GPS acceleration."
269  accel_float.x += ahrs_impl.gps_acceleration; // Longitudinal acceleration
270 #endif
271  accel_float.y += ahrs_impl.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
272  accel_float.z -= ahrs_impl.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
273  }
274  else
275  {
276  ahrs_impl.gps_speed = 0;
278  ahrs_impl.gps_age = 100;
279  }
280 
282 }
283 
284 
285 void ahrs_update_mag(void)
286 {
287 #if USE_MAGNETOMETER
288 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
289 
290  float cos_roll;
291  float sin_roll;
292  float cos_pitch;
293  float sin_pitch;
294 
295  cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi);
296  sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi);
297  cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta);
298  sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta);
299 
300 
301  // Pitch&Roll Compensation:
302  MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
303  MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;
304 
305 /*
306  *
307  // Magnetic Heading
308  Heading = atan2(-Head_Y,Head_X);
309 
310  // Declination correction (if supplied)
311  if( declination != 0.0 )
312  {
313  Heading = Heading + declination;
314  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
315  Heading -= (2.0 * M_PI);
316  else if (Heading < -M_PI)
317  Heading += (2.0 * M_PI);
318  }
319 
320  // Optimization for external DCM use. Calculate normalized components
321  Heading_X = cos(Heading);
322  Heading_Y = sin(Heading);
323 */
324 
325  struct FloatVect3 ltp_mag;
326 
327  ltp_mag.x = MAG_Heading_X;
328  ltp_mag.y = MAG_Heading_Y;
329 
330 #if FLOAT_DCM_SEND_DEBUG
331  // Downlink
332  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
333 #endif
334 
335  // Magnetic Heading
336  // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
337 #endif
338 }
339 
340 void Normalize(void)
341 {
342  float error=0;
343  float temporary[3][3];
344  float renorm=0;
345  uint8_t problem=FALSE;
346 
347  // Find the non-orthogonality of X wrt Y
348  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
349 
350  // Add half the XY error to X, and half to Y
351  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
352  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
353  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
354  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
355 
356  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
357  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
358 
359  // Normalize lenght of X
360  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
361  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
362  // b) if the norm is further from 1, use a real sqrt
363  // c) norm is huge: disaster! reset! mayday!
364  if (renorm < 1.5625f && renorm > 0.64f) {
365  renorm= .5 * (3-renorm); //eq.21
366  } else if (renorm < 100.0f && renorm > 0.01f) {
367  renorm= 1. / sqrt(renorm);
368 #if PERFORMANCE_REPORTING == 1
370 #endif
371  } else {
372  problem = TRUE;
373 #if PERFORMANCE_REPORTING == 1
375 #endif
376  }
377  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
378 
379  // Normalize lenght of Y
380  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
381  if (renorm < 1.5625f && renorm > 0.64f) {
382  renorm= .5 * (3-renorm); //eq.21
383  } else if (renorm < 100.0f && renorm > 0.01f) {
384  renorm= 1. / sqrt(renorm);
385 #if PERFORMANCE_REPORTING == 1
387 #endif
388  } else {
389  problem = TRUE;
390 #if PERFORMANCE_REPORTING == 1
392 #endif
393  }
394  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
395 
396  // Normalize lenght of Z
397  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
398  if (renorm < 1.5625f && renorm > 0.64f) {
399  renorm= .5 * (3-renorm); //eq.21
400  } else if (renorm < 100.0f && renorm > 0.01f) {
401  renorm= 1. / sqrt(renorm);
402 #if PERFORMANCE_REPORTING == 1
404 #endif
405  } else {
406  problem = TRUE;
407 #if PERFORMANCE_REPORTING == 1
409 #endif
410  }
411  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
412 
413  // Reset on trouble
414  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
416  problem = FALSE;
417  }
418 }
419 
420 
422 {
423  //Compensation the Roll, Pitch and Yaw drift.
424  static float Scaled_Omega_P[3];
425  static float Scaled_Omega_I[3];
426  float Accel_magnitude;
427  float Accel_weight;
428  float Integrator_magnitude;
429 
430  // Local Working Variables
431  float errorRollPitch[3];
432  float errorYaw[3];
433  float errorCourse;
434 
435  //*****Roll and Pitch***************
436 
437  // Calculate the magnitude of the accelerometer vector
438  Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
439  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
440  // Dynamic weighting of accelerometer info (reliability filter)
441  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
442  Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
443 
444 #if USE_HIGH_ACCEL_FLAG
445  // Test for high acceleration:
446  // - low speed
447  // - high thrust
448  if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
450  } else {
453  high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
454  }
455  if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
456  high_accel_done = FALSE; // Activate high accel after landing
457  }
458  }
459  if (high_accel_flag) { Accel_weight = 0.; }
460 #endif
461 
462 
463  #if PERFORMANCE_REPORTING == 1
464  {
465 
466  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
467  imu_health += tempfloat;
468  Bound(imu_health,129,65405);
469  }
470  #endif
471 
472  Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
473  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
474 
475  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
476  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
477 
478  //*****YAW***************
479 
480 #if USE_MAGNETOMETER
481  // We make the gyro YAW drift correction based on compass magnetic heading
482 // float mag_heading_x = cos(MAG_Heading);
483 // float mag_heading_y = sin(MAG_Heading);
484  // 2D dot product
485  errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
486  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
487 
488  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
489  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
490 
491  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
492  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
493 
494 #else // Use GPS Ground course to correct yaw gyro drift
495 
497  float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
498  float COGX = cosf(course); //Course overground X axis
499  float COGY = sinf(course); //Course overground Y axis
500 
501  errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
502  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
503 
504  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
505  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
506 
507  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
508  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
509  }
510 #if USE_MAGNETOMETER_ONGROUND == 1
511 #pragma message AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight
512  else if (launch == FALSE)
513  {
514  float COGX = imu.mag.x; // Non-Tilt-Compensated (for filter stability reasons)
515  float COGY = imu.mag.y; // Non-Tilt-Compensated (for filter stability reasons)
516 
517  errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
518  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
519 
520  // P only
521  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW / 10.0);
522  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.fi
523  }
524 #endif // USE_MAGNETOMETER_ONGROUND
525 #endif
526 
527  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
528  Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
529  if (Integrator_magnitude > RadOfDeg(300)) {
530  Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude);
531  }
532 
533 
534 }
535 /**************************************************/
536 
537 void Matrix_update(void)
538 {
539  Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term
540  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
541 
542  #if OUTPUTMODE==1 // With corrected data (drift correction)
543  Update_Matrix[0][0]=0;
544  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
545  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
546  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
547  Update_Matrix[1][1]=0;
548  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
549  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
550  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
551  Update_Matrix[2][2]=0;
552  #else // Uncorrected data (no drift correction)
553  Update_Matrix[0][0]=0;
557  Update_Matrix[1][1]=0;
561  Update_Matrix[2][2]=0;
562  #endif
563 
565 
566  for(int x=0; x<3; x++) //Matrix Addition (update)
567  {
568  for(int y=0; y<3; y++)
569  {
571  }
572  }
573 }
574 
575 /*
576  * Compute body orientation and rates from imu orientation and rates
577  */
578 static inline void compute_body_orientation_and_rates(void) {
579 
586 
587 }
588 
589 static inline void compute_ahrs_representations(void) {
590 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
591  ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
592  ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
594 #else
595  ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
597  ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
598  ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
599 #endif
600 
601  /* set quaternion and rotation matrix representations as well */
604 
606 
607  /*
608  RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
609  &(DCM_Matrix[0][0]),
610  &(DCM_Matrix[0][1]),
611  &(DCM_Matrix[0][2]),
612 
613  &(DCM_Matrix[1][0]),
614  &(DCM_Matrix[1][1]),
615  &(DCM_Matrix[1][2]),
616 
617  &(DCM_Matrix[2][0]),
618  &(DCM_Matrix[2][1]),
619  &(DCM_Matrix[2][2])
620 
621  ));
622  */
623 }
624 
625 #ifdef AHRS_UPDATE_FW_ESTIMATOR
626 void ahrs_update_fw_estimator( void ) {
627  // export results to estimator
631 
635 }
636 #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
bool_t launch
Definition: sim_ap.c:41
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 gps_acceleration
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
uint8_t gps_age
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:48
#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)
bool_t gps_course_valid
#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)