Paparazzi UAS  v5.15_devel-111-g8fb4629
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_dcm.c
Go to the documentation of this file.
1 /*
2  * Released under Creative Commons License
3  *
4  * 2010 The Paparazzi Team
5  *
6  *
7  * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
8  * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
9  *
10  * Modified at Hochschule Bremen, Germany
11  * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
12  *
13  */
14 
28 #include "std.h"
29 
32 #include "autopilot.h" // launch detection
33 
36 
37 #if USE_GPS
38 #include "subsystems/gps.h"
39 #endif
40 
41 #include <string.h>
42 
43 #include "led.h"
44 
45 #if FLOAT_DCM_SEND_DEBUG
46 // FIXME Debugging Only
47 #include "mcu_periph/uart.h"
48 #include "pprzlink/messages.h"
50 #endif
51 
53 
54 // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
55 // Positive pitch : nose up
56 // Positive roll : right wing down
57 // Positive yaw : clockwise
58 
59 struct FloatVect3 accel_float = {0, 0, 0};
60 struct FloatVect3 mag_float = {0, 0, 0};
61 
62 float Omega_Vector[3] = {0, 0, 0}; //Corrected Gyro_Vector data
63 float Omega_P[3] = {0, 0, 0}; //Omega Proportional correction
64 float Omega_I[3] = {0, 0, 0}; //Omega Integrator
65 float Omega[3] = {0, 0, 0};
66 
67 float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
68 float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; //Gyros here
69 float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
70 
71 #if USE_MAGNETOMETER
72 float MAG_Heading_X = 1;
73 float MAG_Heading_Y = 0;
74 #endif
75 
76 static void compute_ahrs_representations(void);
77 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
78 
79 void Normalize(void);
80 void Drift_correction(void);
81 void Matrix_update(float dt);
82 
83 #if PERFORMANCE_REPORTING == 1
84 int renorm_sqrt_count = 0;
85 int renorm_blowup_count = 0;
86 float imu_health = 0.;
87 #endif
88 
89 
90 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
91 {
92  for (int i = 0; i < 3; i++) {
93  for (int j = 0; j < 3; j++) {
94  DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
95  }
96  }
97 }
98 
99 void ahrs_dcm_init(void)
100 {
102  ahrs_dcm.is_aligned = false;
103 
104  /* init ltp_to_imu euler with zero */
106 
108 
109  /* set inital filter dcm */
111 
112  ahrs_dcm.gps_speed = 0;
114  ahrs_dcm.gps_course = 0;
115  ahrs_dcm.gps_course_valid = false;
116  ahrs_dcm.gps_age = 100;
117 }
118 
119 bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
120  struct FloatVect3 *lp_mag)
121 {
122  /* Compute an initial orientation using euler angles */
124 
125  /* Convert initial orientation in quaternion and rotation matrice representations. */
126  struct FloatRMat ltp_to_imu_rmat;
127  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
128 
129  /* set filter dcm */
130  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
131 
132  /* use averaged gyro as initial value for bias */
133  ahrs_dcm.gyro_bias = *lp_gyro;
134 
136  ahrs_dcm.is_aligned = true;
137 
138  return true;
139 }
140 
141 
142 void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
143 {
144  /* unbias rate measurement */
146 
147  /* Uncouple Motions */
148 #ifdef IMU_GYRO_P_Q
149  float dp = 0, dq = 0, dr = 0;
150  dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
151  dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
152  dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
153  dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
154  dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
155  dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
156 
157  ahrs_dcm.imu_rate.p += dp;
158  ahrs_dcm.imu_rate.q += dq;
159  ahrs_dcm.imu_rate.r += dr;
160 #endif
161 
162  Matrix_update(dt);
163 
164  Normalize();
165 
167 }
168 
169 void ahrs_dcm_update_gps(struct GpsState *gps_s)
170 {
171  static float last_gps_speed_3d = 0;
172 
173 #if USE_GPS
174  if (gps_s->fix >= GPS_FIX_3D) {
175  ahrs_dcm.gps_age = 0;
176  ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
177 
178  if (gps_s->gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s
179  ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
180  ahrs_dcm.gps_course_valid = true;
181  } else {
182  ahrs_dcm.gps_course_valid = false;
183  }
184  } else {
185  ahrs_dcm.gps_age = 100;
186  }
187 #endif
188 
189  ahrs_dcm.gps_acceleration += (((ahrs_dcm.gps_speed - last_gps_speed_3d) * 4.0f) - ahrs_dcm.gps_acceleration) / 5.0f;
190  last_gps_speed_3d = ahrs_dcm.gps_speed;
191 }
192 
193 
195 {
196  // DCM filter uses g-force as positive
197  // accelerometer measures [0 0 -g] in a static case
198  accel_float.x = -accel->x;
199  accel_float.y = -accel->y;
200  accel_float.z = -accel->z;
201 
202 
203  ahrs_dcm.gps_age ++;
204  if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
205 #if USE_AHRS_GPS_ACCELERATIONS
206  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
207  accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
208 #endif
209  accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
210  accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
211  } else {
212  ahrs_dcm.gps_speed = 0;
214  ahrs_dcm.gps_age = 100;
215  }
216 
218 }
219 
220 
222 {
223 #if USE_MAGNETOMETER
224 MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
225 
226  float cos_roll;
227  float sin_roll;
228  float cos_pitch;
229  float sin_pitch;
230 
231  cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
232  sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
233  cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
234  sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
235 
236 
237  // Pitch&Roll Compensation:
238  MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
239  MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;
240 
241  /*
242  *
243  // Magnetic Heading
244  Heading = atan2(-Head_Y,Head_X);
245 
246  // Declination correction (if supplied)
247  if( declination != 0.0 )
248  {
249  Heading = Heading + declination;
250  if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
251  Heading -= (2.0 * M_PI);
252  else if (Heading < -M_PI)
253  Heading += (2.0 * M_PI);
254  }
255 
256  // Optimization for external DCM use. Calculate normalized components
257  Heading_X = cos(Heading);
258  Heading_Y = sin(Heading);
259  */
260 
261 #if FLOAT_DCM_SEND_DEBUG
262  struct FloatVect3 ltp_mag;
263 
264  ltp_mag.x = MAG_Heading_X;
265  ltp_mag.y = MAG_Heading_Y;
266 
267  // Downlink
268  RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
269 #endif
270 
271  // Magnetic Heading
272  // MAG_Heading = atan2(mag->y, -mag->x);
273 
274 #else // !USE_MAGNETOMETER
275  // get rid of unused param warning...
276  mag = mag;
277 #endif
278 }
279 
280 void Normalize(void)
281 {
282  float error = 0;
283  float temporary[3][3];
284  float renorm = 0;
285  uint8_t problem = false;
286 
287  // Find the non-orthogonality of X wrt Y
288  error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
289 
290  // Add half the XY error to X, and half to Y
291  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
292  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
293  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
294  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
295 
296  // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
297  Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
298 
299  // Normalize lenght of X
300  renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
301  // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
302  // b) if the norm is further from 1, use a real sqrt
303  // c) norm is huge: disaster! reset! mayday!
304  if (renorm < 1.5625f && renorm > 0.64f) {
305  renorm = .5 * (3 - renorm); //eq.21
306  } else if (renorm < 100.0f && renorm > 0.01f) {
307  renorm = 1. / sqrtf(renorm);
308 #if PERFORMANCE_REPORTING == 1
310 #endif
311  } else {
312  problem = true;
313 #if PERFORMANCE_REPORTING == 1
315 #endif
316  }
317  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
318 
319  // Normalize lenght of Y
320  renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
321  if (renorm < 1.5625f && renorm > 0.64f) {
322  renorm = .5 * (3 - renorm); //eq.21
323  } else if (renorm < 100.0f && renorm > 0.01f) {
324  renorm = 1. / sqrtf(renorm);
325 #if PERFORMANCE_REPORTING == 1
327 #endif
328  } else {
329  problem = true;
330 #if PERFORMANCE_REPORTING == 1
332 #endif
333  }
334  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
335 
336  // Normalize lenght of Z
337  renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
338  if (renorm < 1.5625f && renorm > 0.64f) {
339  renorm = .5 * (3 - renorm); //eq.21
340  } else if (renorm < 100.0f && renorm > 0.01f) {
341  renorm = 1. / sqrtf(renorm);
342 #if PERFORMANCE_REPORTING == 1
344 #endif
345  } else {
346  problem = true;
347 #if PERFORMANCE_REPORTING == 1
349 #endif
350  }
351  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
352 
353  // Reset on trouble
354  if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
356  problem = false;
357  }
358 }
359 
360 // strong structural vibrations can prevent to perform the drift correction
361 // so accel magnitude is filtered before computing the weighting heuristic
362 #ifndef ACCEL_WEIGHT_FILTER
363 #define ACCEL_WEIGHT_FILTER 8
364 #endif
365 
366 // the weigthing is function of the length of a band of 1G by default
367 // so <0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0
368 // adjust the band size if needed, the value should be >0
369 #ifndef ACCEL_WEIGHT_BAND
370 #define ACCEL_WEIGHT_BAND 1.f
371 #endif
372 
374 {
375  //Compensation the Roll, Pitch and Yaw drift.
376  static float Scaled_Omega_P[3];
377  static float Scaled_Omega_I[3];
378  static float Accel_filtered = 0.f;
379  float Accel_magnitude;
380  float Accel_weight;
381  float Integrator_magnitude;
382 
383  // Local Working Variables
384  float errorRollPitch[3];
385  float errorYaw[3];
386  float errorCourse;
387 
388  //*****Roll and Pitch***************
389 
390  // Calculate the magnitude of the accelerometer vector
391  Accel_magnitude = sqrtf(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
392  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
393 #if ACCEL_WEIGHT_FILTER
394  Accel_filtered = (Accel_magnitude + (ACCEL_WEIGHT_FILTER - 1) * Accel_filtered) / ACCEL_WEIGHT_FILTER;
395 #else // set ACCEL_WEIGHT_FILTER to 0 to disable filter
396  Accel_filtered = Accel_magnitude;
397 #endif
398  // Dynamic weighting of accelerometer info (reliability filter)
399  // Weight for accelerometer info according to band size (min value is 0.1 to prevent division by zero)
400  Accel_weight = Clip(1.f - (2.f / Max(0.1f,ACCEL_WEIGHT_BAND)) * fabsf(1.f - Accel_filtered), 0.f, 1.f);
401 
402 
403 #if PERFORMANCE_REPORTING == 1
404  {
405  //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
406  float tempfloat = ((Accel_weight - 0.5) * 256.0f);
407  imu_health += tempfloat;
408  Bound(imu_health, 129, 65405);
409  }
410 #endif
411 
412  Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
413  Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
414 
415  Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
416  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
417 
418  //*****YAW***************
419 
420 #if USE_MAGNETOMETER
421  // We make the gyro YAW drift correction based on compass magnetic heading
422 // float mag_heading_x = cos(MAG_Heading);
423 // float mag_heading_y = sin(MAG_Heading);
424  // 2D dot product
425  //Calculating YAW error
426  errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
427  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
428  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
429 
430  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
431  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
432 
433  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
434  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
435 
436 #else // Use GPS Ground course to correct yaw gyro drift
437 
439  float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
440  float COGX = cosf(course); //Course overground X axis
441  float COGY = sinf(course); //Course overground Y axis
442 
443  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
444  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
445  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
446 
447  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
448  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
449 
450  Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
451  Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
452  }
453 #if USE_MAGNETOMETER_ONGROUND
454  PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
455  else if (autopilot.launch == FALSE) {
456  float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
457  float COGY = mag->y; // Non-Tilt-Compensated (for filter stability reasons)
458 
459  errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
460  //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
461  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
462 
463  // P only
464  Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
465  Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
466  }
467 #endif // USE_MAGNETOMETER_ONGROUND
468 #endif
469 
470  // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
471  Integrator_magnitude = sqrtf(Vector_Dot_Product(Omega_I, Omega_I));
472  if (Integrator_magnitude > RadOfDeg(300)) {
473  Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
474  }
475 
476 
477 }
478 /**************************************************/
479 
480 void Matrix_update(float dt)
481 {
482  Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
483  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
484 
485 #if OUTPUTMODE==1 // With corrected data (drift correction)
486  Update_Matrix[0][0] = 0;
487  Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
488  Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
489  Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
490  Update_Matrix[1][1] = 0;
491  Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
492  Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
493  Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
494  Update_Matrix[2][2] = 0;
495 #else // Uncorrected data (no drift correction)
496  Update_Matrix[0][0] = 0;
497  Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
498  Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
499  Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
500  Update_Matrix[1][1] = 0;
501  Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
502  Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
503  Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
504  Update_Matrix[2][2] = 0;
505 #endif
506 
508 
509  for (int x = 0; x < 3; x++) { //Matrix Addition (update)
510  for (int y = 0; y < 3; y++) {
511  DCM_Matrix[x][y] += Temporary_Matrix[x][y];
512  }
513  }
514 }
515 
517 {
518 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
519  ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
520  ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
522 #else
523  ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
524  ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
525  ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
526  ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
527 #endif
528 }
529 
531 {
533 }
534 
536 {
538 
539  if (!ahrs_dcm.is_aligned) {
540  /* Set ltp_to_imu so that body is zero */
542  }
543 }
bool launch
request launch
Definition: autopilot.h:71
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float phi
in radians
#define float_rmat_of_eulers
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
static void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
void Matrix_update(float dt)
struct FloatVect3 accel_float
#define FLOAT_EULERS_ZERO(_e)
#define Kp_YAW
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
float r
in rad/s
void ahrs_dcm_init(void)
struct FloatVect3 mag_float
Utility functions for floating point AHRS implementations.
#define Ki_ROLLPITCH
#define FLOAT_RATES_ZERO(_r)
#define Ki_YAW
float psi
in radians
#define ACCEL_WEIGHT_BAND
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
float DCM_Matrix[3][3]
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
void Drift_correction(void)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float q
in rad/s
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
float p
in rad/s
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct FloatVect3 *accel, struct FloatVect3 *mag)
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
float Update_Matrix[3][3]
#define FALSE
Definition: std.h:5
Roation quaternion.
float imu_health
int renorm_sqrt_count
struct FloatEulers ltp_to_imu_euler
float theta
in radians
void Normalize(void)
float Omega_Vector[3]
static float Vector_Dot_Product(float vector1[3], float vector2[3])
float Omega[3]
Paparazzi floating point algebra.
data structure for GPS information
Definition: gps.h:87
struct AhrsFloatDCM ahrs_dcm
bool gps_course_valid
Device independent GPS code (interface)
Algebra helper functions for DCM.
int renorm_blowup_count
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct FloatRates gyro_bias
void ahrs_dcm_update_gps(struct GpsState *gps_s)
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
#define ACCEL_WEIGHT_FILTER
#define Max(x, y)
Definition: main_fbw.c:53
enum AhrsDCMStatus status
Core autopilot interface common to all firmwares.
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
float MAG_Heading_Y
struct OrientationReps body_to_imu
unsigned char uint8_t
Definition: types.h:14
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
static int16_t course[3]
Definition: airspeed_uADC.c:57
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
float Temporary_Matrix[3][3]
uint8_t gps_age
void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
rotation matrix
arch independent LED (Light Emitting Diodes) API
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
float Omega_I[3]
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
Attitude estimation for fixedwings based on the DCM.
#define GRAVITY
static void compute_ahrs_representations(void)
void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
float MAG_Heading_X
uint8_t fix
status of fix
Definition: gps.h:107
float gps_acceleration
float Omega_P[3]
angular rates
#define Kp_ROLLPITCH
struct FloatRates imu_rate
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).