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