Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_cmpl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2013 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
33 #include "math/pprz_algebra_int.h"
35 #include "generated/airframe.h"
36 #if USE_GPS
37 #include "subsystems/gps.h"
38 #endif
39 
40 //#include "../../test/pprz_algebra_print.h"
41 
42 #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
43 #error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
44 #endif
45 #if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
46 #error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
47 #endif
48 
49 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
50 #warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
51 #endif
52 
53 /*
54  * default gains for correcting attitude and bias from accel/mag
55  */
56 #ifndef AHRS_ACCEL_OMEGA
57 #define AHRS_ACCEL_OMEGA 0.063
58 #endif
59 #ifndef AHRS_ACCEL_ZETA
60 #define AHRS_ACCEL_ZETA 0.9
61 #endif
62 
63 #ifndef AHRS_MAG_OMEGA
64 #define AHRS_MAG_OMEGA 0.04
65 #endif
66 #ifndef AHRS_MAG_ZETA
67 #define AHRS_MAG_ZETA 0.9
68 #endif
69 
71 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
72 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
73 #endif
74 
75 
76 void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt);
77 void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt);
78 void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag);
79 
81 
82 void ahrs_fc_init(void)
83 {
85  ahrs_fc.is_aligned = false;
86 
88  ahrs_fc.heading_aligned = false;
89 
90  /* init ltp_to_imu quaternion as zero/identity rotation */
92 
95 
97 
98  /* set default filter cut-off frequency and damping */
103 
104 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
105  ahrs_fc.correct_gravity = true;
106 #else
107  ahrs_fc.correct_gravity = false;
108 #endif
109 
111 
112  VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
113 
114  ahrs_fc.accel_cnt = 0;
115  ahrs_fc.mag_cnt = 0;
116 }
117 
118 bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
119  struct FloatVect3 *lp_mag __attribute__((unused)))
120 {
121 
122 #if USE_MAGNETOMETER
123  /* Compute an initial orientation from accel and mag directly as quaternion */
125  ahrs_fc.heading_aligned = true;
126 #else
127  /* Compute an initial orientation from accel and just set heading to zero */
129  ahrs_fc.heading_aligned = false;
130 #endif
131 
132  /* Convert initial orientation from quat to rotation matrix representations. */
134 
135  /* used averaged gyro as initial value for bias */
136  ahrs_fc.gyro_bias = *lp_gyro;
137 
139  ahrs_fc.is_aligned = true;
140 
141  return true;
142 }
143 
144 
145 void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
146 {
147 
148  struct FloatRates rates = *gyro;
149  /* unbias measurement */
150  RATES_SUB(rates, ahrs_fc.gyro_bias);
151 
152 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
153  const float alpha = 0.1;
154  FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), rates, alpha);
155 #else
156  RATES_COPY(ahrs_fc.imu_rate, rates);
157 #endif
158 
159  /* add correction */
160  struct FloatRates omega;
161  RATES_SUM(omega, rates, ahrs_fc.rate_correction);
162  /* and zeros it */
164 
165 #if AHRS_PROPAGATE_RMAT
169 #endif
170 #if AHRS_PROPAGATE_QUAT
174 #endif
175 
176  // increase accel and mag propagation counters
177  ahrs_fc.accel_cnt++;
178  ahrs_fc.mag_cnt++;
179 }
180 
181 void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
182 {
183  // check if we had at least one propagation since last update
184  if (ahrs_fc.accel_cnt == 0) {
185  return;
186  }
187 
188  /* last column of roation matrix = ltp z-axis in imu-frame */
189  struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
192  };
193 
194  struct FloatVect3 imu_accel = *accel;
195  struct FloatVect3 residual;
196  struct FloatVect3 pseudo_gravity_measurement;
197 
199  /*
200  * centrifugal acceleration in body frame
201  * a_c_body = omega x (omega x r)
202  * (omega x r) = tangential velocity in body frame
203  * a_c_body = omega x vel_tangential_body
204  * assumption: tangential velocity only along body x-axis (or negative z-axis)
205  */
206  const struct FloatVect3 vel_tangential_body =
207 #if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
208  /* AHRS_GRAVITY_UPDATE_COORDINATED_TURN assumes the GPS speed is in the X axis direction.
209  * Quadshot, DelftaCopter and other hybrids can have the GPS speed in the negative Z direction
210  */
211  {0.0, 0.0, -ahrs_fc.ltp_vel_norm);
212 #else
213  /* assume tangential velocity along body x-axis */
214  {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
215 #endif
216  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
217  struct FloatRates body_rate;
218  float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
219  struct FloatVect3 acc_c_body;
220  VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
221 
222  /* convert centrifugal acceleration from body to imu frame */
223  struct FloatVect3 acc_c_imu;
224  float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
225 
226  /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
227  VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_imu);
228 
229  } else {
230  VECT3_COPY(pseudo_gravity_measurement, imu_accel);
231  }
232 
233  VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
234 
235  /* FIR filtered pseudo_gravity_measurement */
236 #define FIR_FILTER_SIZE 8
237  static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
238  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
239  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
240  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
241 
243  /* heuristic on acceleration (gravity estimate) norm */
244  /* Factor how strongly to change the weight.
245  * e.g. for gravity_heuristic_factor 30:
246  * <0.66G = 0, 1G = 1.0, >1.33G = 0
247  */
248 
249  const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
250  ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
251  Bound(ahrs_fc.weight, 0.15, 1.0);
252  } else {
253  ahrs_fc.weight = 1.0;
254  }
255 
256  /* Complementary filter proportional gain.
257  * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
258  * with ahrs_fc.accel_cnt beeing the number of propagations since last update
259  */
260  const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
261  ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
262  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
263 
264  // reset accel propagation counter
265  ahrs_fc.accel_cnt = 0;
266 
267  /* Complementary filter integral gain
268  * Correct the gyro bias.
269  * Ki = (omega*weight)^2 * dt
270  */
271  const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
272  ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
273  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
274 
275  /* FIXME: saturate bias */
276 }
277 
278 
279 void ahrs_fc_update_mag(struct FloatVect3 *mag __attribute__((unused)), float dt __attribute__((unused)))
280 {
281 #if USE_MAGNETOMETER
282  // check if we had at least one propagation since last update
283  if (ahrs_fc.mag_cnt == 0) {
284  return;
285  }
286 #if AHRS_MAG_UPDATE_ALL_AXES
287  ahrs_fc_update_mag_full(mag, dt);
288 #else
289  ahrs_fc_update_mag_2d(mag, dt);
290 #endif
291  // reset mag propagation counter
292  ahrs_fc.mag_cnt = 0;
293 #endif
294 }
295 
296 void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
297 {
298 
299  struct FloatVect3 expected_imu;
301 
302  struct FloatVect3 measured_imu = *mag;
303  struct FloatVect3 residual_imu;
304  VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
305  // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
306  // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
307  // DISPLAY_FLOAT_VECT3("# residual", residual);
308 
309  /* Complementary filter proportional gain.
310  * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
311  * with ahrs_fc.mag_cnt beeing the number of propagations since last update
312  */
313 
314  const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
315  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
316 
317  /* Complementary filter integral gain
318  * Correct the gyro bias.
319  * Ki = (omega*weight)^2 * dt
320  */
321  const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
322  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
323 
324 }
325 
326 void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
327 {
328 
329  struct FloatVect2 expected_ltp;
330  VECT2_COPY(expected_ltp, ahrs_fc.mag_h);
331  // normalize expected ltp in 2D (x,y)
332  float_vect2_normalize(&expected_ltp);
333 
334  struct FloatVect3 measured_imu = *mag;
335  struct FloatVect3 measured_ltp;
336  float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu);
337 
338  struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
339 
340  // normalize measured ltp in 2D (x,y)
341  float_vect2_normalize(&measured_ltp_2d);
342 
343  struct FloatVect3 residual_ltp = {
344  0,
345  0,
346  measured_ltp_2d.x *expected_ltp.y - measured_ltp_2d.y * expected_ltp.x
347  };
348 
349  // printf("res : %f\n", residual_ltp.z);
350 
351  struct FloatVect3 residual_imu;
352  float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
353 
354 
355  /* Complementary filter proportional gain.
356  * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
357  * with ahrs_fc.mag_cnt beeing the number of propagations since last update
358  */
359  const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
360  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
361 
362  /* Complementary filter integral gain
363  * Correct the gyro bias.
364  * Ki = (omega*weight)^2 * dt
365  */
366  const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
367  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
368 }
369 
370 
372 {
373 
374  /* project mag on local tangeant plane */
375  struct FloatEulers ltp_to_imu_euler;
376  float_eulers_of_rmat(&ltp_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
377 
378  const float cphi = cosf(ltp_to_imu_euler.phi);
379  const float sphi = sinf(ltp_to_imu_euler.phi);
380  const float ctheta = cosf(ltp_to_imu_euler.theta);
381  const float stheta = sinf(ltp_to_imu_euler.theta);
382  const float mn = ctheta * mag->x + sphi * stheta * mag->y + cphi * stheta * mag->z;
383  const float me = 0. * mag->x + cphi * mag->y - sphi * mag->z;
384 
385  const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me +
386  RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
387  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
390  };
391  const float mag_rate_update_gain = 2.5;
392  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
393  const float mag_bias_update_gain = -2.5e-4;
394  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));
395 
396 }
397 
398 void ahrs_fc_update_gps(struct GpsState *gps_s __attribute__((unused)))
399 {
400 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
401  if (gps_s->fix >= GPS_FIX_3D) {
402  ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.;
404  } else {
405  ahrs_fc.ltp_vel_norm_valid = false;
406  }
407 #endif
408 
409 #if AHRS_USE_GPS_HEADING && USE_GPS
410  //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
411  if (gps_s->fix >= GPS_FIX_3D && gps_s->gspeed >= 500 &&
412  gps_s->cacc <= RadOfDeg(10 * 1e7)) {
413 
414  // gps_s->course is in rad * 1e7, we need it in rad
415  float course = gps_s->course / 1e7;
416 
417  if (ahrs_fc.heading_aligned) {
418  /* the assumption here is that there is no side-slip, so heading=course */
419  ahrs_fc_update_heading(course);
420  } else {
421  /* hard reset the heading if this is the first measurement */
422  ahrs_fc_realign_heading(course);
423  }
424  }
425 #endif
426 }
427 
428 
430 {
431 
432  FLOAT_ANGLE_NORMALIZE(heading);
433 
435  struct FloatRMat *ltp_to_body_rmat = orientationGetRMat_f(&ahrs_fc.ltp_to_body);
436 
437  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
438  // we only consider x and y
439  struct FloatVect2 expected_ltp = {
440  RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
441  RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
442  };
443 
444  // expected_heading cross measured_heading
445  struct FloatVect3 residual_ltp = {
446  0,
447  0,
448  expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
449  };
450 
451  struct FloatVect3 residual_imu;
452  float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
453 
454  const float heading_rate_update_gain = 2.5;
455  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
456 
457  float heading_bias_update_gain;
458  /* crude attempt to only update bias if deviation is small
459  * e.g. needed when you only have gps providing heading
460  * and the inital heading is totally different from
461  * the gps course information you get once you have a gps fix.
462  * Otherwise the bias will be falsely "corrected".
463  */
464  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
465  heading_bias_update_gain = -2.5e-4;
466  } else {
467  heading_bias_update_gain = 0.0;
468  }
469  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
470 }
471 
472 
474 {
475  FLOAT_ANGLE_NORMALIZE(heading);
476 
477  /* quaternion representing only the heading rotation from ltp to body */
478  struct FloatQuat q_h_new;
479  q_h_new.qx = 0.0;
480  q_h_new.qy = 0.0;
481  q_h_new.qz = sinf(heading / 2.0);
482  q_h_new.qi = cosf(heading / 2.0);
483 
485  struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
486 
487  /* quaternion representing current heading only */
488  struct FloatQuat q_h = *ltp_to_body_quat;
489  q_h.qx = 0.;
490  q_h.qy = 0.;
491  float_quat_normalize(&q_h);
492 
493  /* quaternion representing rotation from current to new heading */
494  struct FloatQuat q_c;
495  float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
496 
497  /* correct current heading in body frame */
498  struct FloatQuat q;
499  float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat);
500 
501  /* compute ltp to imu rotation quaternion and matrix */
502  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
503  float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
505 
506  ahrs_fc.heading_aligned = true;
507 }
508 
510 {
512 }
513 
515 {
517 
518  if (!ahrs_fc.is_aligned) {
519  /* Set ltp_to_imu so that body is zero */
522  }
523 }
524 
526 {
527  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
528  struct FloatQuat ltp_to_body_quat;
529  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
530  orientationSetQuat_f(&ahrs_fc.ltp_to_body, &ltp_to_body_quat);
531 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
#define FLOAT_ANGLE_NORMALIZE(_a)
void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
uint16_t mag_cnt
number of propagations since last mag update
float accel_omega
filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) ...
Simple matrix helper macros.
float phi
in radians
struct FloatRates gyro_bias
static void orientationSetIdentity(struct OrientationReps *orientation)
Set to identity orientation.
#define AHRS_MAG_OMEGA
#define AHRS_MAG_ZETA
bool correct_gravity
enable gravity correction during coordinated turns
struct FloatRMat ltp_to_imu_rmat
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
float alpha
Definition: textons.c:107
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
Utility functions for floating point AHRS implementations.
#define FLOAT_RATES_ZERO(_r)
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ahrs_fc_update_heading(float heading)
Update yaw based on a heading measurement.
void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:351
void ahrs_fc_update_mag(struct FloatVect3 *mag, float dt)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
void ahrs_fc_init(void)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
struct FloatQuat ltp_to_imu_quat
euler angles
Roation quaternion.
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
static float float_vect3_norm(struct FloatVect3 *v)
float theta
in radians
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
static float heading
Definition: ahrs_infrared.c:45
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
Paparazzi floating point algebra.
data structure for GPS information
Definition: gps.h:81
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Device independent GPS code (interface)
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:358
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
#define RATES_ADD_SCALED_VECT(_ro, _v, _s)
Definition: pprz_algebra.h:419
static void float_quat_normalize(struct FloatQuat *q)
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
float accel_zeta
filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) ...
#define AHRS_ACCEL_OMEGA
struct FloatVect3 mag_h
void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
void ahrs_fc_realign_heading(float heading)
Hard reset yaw to a heading.
void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
float float_rmat_reorthogonalize(struct FloatRMat *rm)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
struct FloatRates imu_rate
#define FIR_FILTER_SIZE
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
static int16_t course[3]
Definition: airspeed_uADC.c:57
void ahrs_fc_update_gps(struct GpsState *gps_s)
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
Complementary filter in float to estimate the attitude, heading and gyro bias.
struct OrientationReps body_to_imu
void ahrs_fc_recompute_ltp_to_body(void)
rotation matrix
uint16_t accel_cnt
number of propagations since last accel update
float ltp_vel_norm
velocity norm for gravity correction during coordinated turns
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
struct FloatRates rate_correction
enum AhrsFCStatus status
float mag_zeta
filter damping for correcting the gyro bias from magnetometer
#define AHRS_ACCEL_ZETA
struct AhrsFloatCmpl ahrs_fc
angular rates
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Paparazzi fixed point algebra.
struct OrientationReps ltp_to_body
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
Definition: pprz_algebra.h:254