Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_int_cmpl_quat.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-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 
31 #include "generated/airframe.h"
32 
35 
36 #if USE_GPS
37 #include "modules/gps/gps.h"
38 #endif
39 #include "math/pprz_trig_int.h"
40 #include "math/pprz_algebra_int.h"
41 
42 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
43 PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
44 #endif
45 
46 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
47 #warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
48 #endif
49 
50 #if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING
51 #warning "Please use either USE_MAGNETOMETER or AHRS_USE_GPS_HEADING."
52 #endif
53 
54 #if AHRS_USE_GPS_HEADING && !USE_GPS
55 #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
56 #endif
57 
58 /*
59  * default gains for correcting attitude and bias from accel/mag
60  */
61 #ifndef AHRS_ACCEL_OMEGA
62 #define AHRS_ACCEL_OMEGA 0.063
63 #endif
64 #ifndef AHRS_ACCEL_ZETA
65 #define AHRS_ACCEL_ZETA 0.9
66 #endif
67 PRINT_CONFIG_VAR(AHRS_ACCEL_OMEGA)
68 PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA)
69 
70 
71 #ifndef AHRS_MAG_OMEGA
72 #define AHRS_MAG_OMEGA 0.04
73 #endif
74 #ifndef AHRS_MAG_ZETA
75 #define AHRS_MAG_ZETA 0.9
76 #endif
77 #if USE_MAGNETOMETER
78 PRINT_CONFIG_VAR(AHRS_MAG_OMEGA)
79 PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
80 #endif
81 
83 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
84 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
85 #endif
86 
88 #ifndef AHRS_BIAS_UPDATE_HEADING_THRESHOLD
89 #define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0
90 #endif
91 
95 #ifndef AHRS_HEADING_UPDATE_GPS_MIN_SPEED
96 #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
97 #endif
98 
100 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
101 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
102 #define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
103 #endif
104 
105 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
106 #define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
107 #endif
108 #endif
109 
111 
112 static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt);
113 static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt);
114 
115 void ahrs_icq_init(void)
116 {
117 
119  ahrs_icq.is_aligned = false;
120 
122  ahrs_icq.heading_aligned = false;
123 
124  /* init ltp_to_body quaternion as zero/identity rotation */
127 
131 
132  /* set default filter cut-off frequency and damping */
139 
140  /* set default gravity heuristic */
142 
143 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
144  ahrs_icq.correct_gravity = true;
145 #else
146  ahrs_icq.correct_gravity = false;
147 #endif
148 
150  MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
151 
152  ahrs_icq.accel_cnt = 0;
153  ahrs_icq.mag_cnt = 0;
154 }
155 
156 
157 bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
158  struct Int32Vect3 *lp_mag)
159 {
160 
161 #if USE_MAGNETOMETER
162  /* Compute an initial orientation from accel and mag directly as quaternion */
164  lp_accel, lp_mag);
165  ahrs_icq.heading_aligned = true;
166 #else
167  /* Compute an initial orientation from accel and just set heading to zero */
169  ahrs_icq.heading_aligned = false;
170  // supress unused arg warning
171  lp_mag = lp_mag;
172 #endif
173 
174  /* Use low passed gyro value as initial bias */
175  RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro);
176  RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
178 
180  ahrs_icq.is_aligned = true;
181 
182  return true;
183 }
184 
185 
186 void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
187 {
188  int32_t freq = (int32_t)(1. / dt);
189 
190  /* unbias gyro */
191  struct Int32Rates omega;
192  RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);
193 
194  /* low pass rate */
195 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
196  RATES_SMUL(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
197  RATES_ADD(ahrs_icq.body_rate, omega);
198  RATES_SDIV(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
199 #else
200  RATES_COPY(ahrs_icq.body_rate, omega);
201 #endif
202 
203  /* add correction */
205  /* and zeros it */
207 
208  /* integrate quaternion */
210  &omega, freq);
212 
213  // increase accel and mag propagation counters
215  ahrs_icq.mag_cnt++;
216 }
217 
218 
220 {
221  /* Complementary filter proportionnal gain (without frequency correction)
222  * Kp = 2 * omega * zeta
223  *
224  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
225  * accel_inv_kp = 4096 * 9.81 / Kp
226  */
227  ahrs_icq.accel_inv_kp = 4096 * 9.81 /
229 
230  /* Complementary filter integral gain
231  * Ki = omega^2
232  *
233  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
234  * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki
235  * accel_inv_ki = 9.81 / 2048 / Ki
236  */
238 }
239 
240 void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
241 {
242  // check if we had at least one propagation since last update
243  if (ahrs_icq.accel_cnt == 0) {
244  return;
245  }
246 
247  // c2 = ltp z-axis in imu-frame
248  struct Int32RMat ltp_to_body_rmat;
249  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
250  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_body_rmat, 0, 2),
251  RMAT_ELMT(ltp_to_body_rmat, 1, 2),
252  RMAT_ELMT(ltp_to_body_rmat, 2, 2)
253  };
254  struct Int32Vect3 residual;
255 
256  struct Int32Vect3 pseudo_gravity_measurement;
257 
259  /*
260  * centrifugal acceleration in body frame
261  * a_c_body = omega x (omega x r)
262  * (omega x r) = tangential velocity in body frame
263  * a_c_body = omega x vel_tangential_body
264  * assumption: tangential velocity only along body x-axis (or negative z-axis)
265  */
266 
267  // FIXME: check overflows !
268 #define COMPUTATION_FRAC 16
269 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
270 
271  const struct Int32Vect3 vel_tangential_body =
272 #if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
273  /* AHRS_GRAVITY_UPDATE_COORDINATED_TURN assumes the GPS speed is in the X axis direction.
274  * Quadshot, DelftaCopter and other hybrids can have the GPS speed in the negative Z direction
275  */
276  {0, 0, -(ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC)};
277 #else
278  /* assume tangential velocity along body x-axis */
280 #endif
281  struct Int32Vect3 acc_c_body;
282  VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_icq.body_rate, vel_tangential_body);
283  INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
284 
285  /* centrifucal acceleration subtract it from imu measurement to get a corrected measurement
286  * of the gravity vector */
287  VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_body);
288  } else {
289  VECT3_COPY(pseudo_gravity_measurement, *accel);
290  }
291 
292  /* compute the residual of the pseudo gravity vector in imu frame */
293  VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
294 
295 
296  /* FIR filtered pseudo_gravity_measurement */
297 #define FIR_FILTER_SIZE 8
298  static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
299  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
300  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
301  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
302 
303 
305  /* heuristic on acceleration (gravity estimate) norm */
306  /* Factor how strongly to change the weight.
307  * e.g. for gravity_heuristic_factor 30:
308  * <0.66G = 0, 1G = 1.0, >1.33G = 0
309  */
310 
311  struct FloatVect3 g_meas_f;
312  ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
313  const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f) / 9.81;
314  ahrs_icq.weight = 1.0 - ahrs_icq.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10;
315  Bound(ahrs_icq.weight, 0.15, 1.0);
316  } else {
317  ahrs_icq.weight = 1.0;
318  }
319 
320  /* Complementary filter proportional gain.
321  * Kp = 2 * zeta * omega
322  * final Kp with frequency correction = Kp * ahrs_icq.accel_cnt
323  * with ahrs_icq.accel_cnt beeing the number of propagations since last update
324  *
325  * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
326  * rate_correction FRAC: RATE_FRAC = 12
327  * FRAC_conversion: 2^12 / 2^24 = 1 / 4096
328  * cross_product_gain : 9.81 m/s2
329  *
330  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
331  * accel_inv_kp = 4096 * 9.81 / Kp
332  *
333  * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain)
334  * inv_rate_scale = 1 / Kp / weight
335  * inv_rate_scale = accel_inv_kp / accel_cnt / weight
336  */
338  / ahrs_icq.weight);
339  Bound(inv_rate_scale, 8192, 4194304);
340 
341  ahrs_icq.rate_correction.p -= residual.x / inv_rate_scale;
342  ahrs_icq.rate_correction.q -= residual.y / inv_rate_scale;
343  ahrs_icq.rate_correction.r -= residual.z / inv_rate_scale;
344 
345  // reset accel propagation counter
346  ahrs_icq.accel_cnt = 0;
347 
348  /* Complementary filter integral gain
349  * Correct the gyro bias.
350  * Ki = omega^2 * dt
351  *
352  * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
353  * high_rez_bias = RATE_FRAC+28 = 40
354  * FRAC_conversion: 2^40 / 2^24 = 2^16
355  * cross_product_gain : 9.81 m/s2
356  *
357  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
358  * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki
359  *
360  * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain)
361  * inv_bias_gain = accel_inv_ki / weight^2
362  */
363 
364  int32_t inv_bias_gain = (int32_t)(ahrs_icq.accel_inv_ki /
365  (dt * ahrs_icq.weight * ahrs_icq.weight));
366  Bound(inv_bias_gain, 8, 65536)
367  ahrs_icq.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
368  ahrs_icq.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
369  ahrs_icq.high_rez_bias.r += (residual.z / inv_bias_gain) << 5;
370 
372 
373 }
374 
375 
376 void ahrs_icq_update_mag(struct Int32Vect3 *mag __attribute__((unused)), float dt __attribute__((unused)))
377 {
378 #if USE_MAGNETOMETER
379  // check if we had at least one propagation since last update
380  if (ahrs_icq.mag_cnt == 0) {
381  return;
382  }
383 #if AHRS_MAG_UPDATE_ALL_AXES
384  ahrs_icq_update_mag_full(mag, dt);
385 #else
386  ahrs_icq_update_mag_2d(mag, dt);
387 #endif
388  // reset mag propagation counter
389  ahrs_icq.mag_cnt = 0;
390 #endif
391 }
392 
394 {
395  /* Complementary filter proportionnal gain = 2*omega*zeta */
397  /* Complementary filter integral gain = omega^2 */
399 }
400 
401 
402 static inline void ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
403 {
404 
405  struct Int32RMat ltp_to_body_rmat;
406  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
407 
408  struct Int32Vect3 expected_imu;
409  int32_rmat_vmult(&expected_imu, &ltp_to_body_rmat, &ahrs_icq.mag_h);
410 
411  struct Int32Vect3 residual;
412  VECT3_CROSS_PRODUCT(residual, *mag, expected_imu);
413 
414  /* Complementary filter proportionnal gain.
415  * Kp = 2 * mag_zeta * mag_omega
416  * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
417  * with ahrs_icq.mag_cnt beeing the number of propagations since last update
418  *
419  * residual FRAC: 2 * MAG_FRAC = 22
420  * rate_correction FRAC: RATE_FRAC = 12
421  * FRAC conversion: 2^12 / 2^22 = 1/1024
422  *
423  * inv_rate_gain = 1 / Kp / FRAC_conversion
424  * inv_rate_gain = 1024 / Kp
425  */
426 
427  const int32_t inv_rate_gain = (int32_t)(1024.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
428 
429  ahrs_icq.rate_correction.p += residual.x / inv_rate_gain;
430  ahrs_icq.rate_correction.q += residual.y / inv_rate_gain;
431  ahrs_icq.rate_correction.r += residual.z / inv_rate_gain;
432 
433  /* Complementary filter integral gain
434  * Correct the gyro bias.
435  * Ki = omega^2 * dt
436  *
437  * residual FRAC: 2* MAG_FRAC = 22
438  * high_rez_bias FRAC: RATE_FRAC+28 = 40
439  * FRAC conversion: 2^40 / 2^22 = 2^18
440  *
441  * bias_gain = Ki * FRAC_conversion = Ki * 2^18
442  */
443  const int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 18));
444 
445  ahrs_icq.high_rez_bias.p -= residual.x * bias_gain;
446  ahrs_icq.high_rez_bias.q -= residual.y * bias_gain;
447  ahrs_icq.high_rez_bias.r -= residual.z * bias_gain;
448 
449 
451 
452 }
453 
454 
455 static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
456 {
457 
458  struct Int32Vect2 expected_ltp = {ahrs_icq.mag_h.x, ahrs_icq.mag_h.y};
459  /* normalize expected ltp in 2D (x,y) */
460  int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC);
461 
462  struct Int32RMat ltp_to_body_rmat;
463  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
464 
465  struct Int32Vect3 measured_ltp;
466  int32_rmat_transp_vmult(&measured_ltp, &ltp_to_body_rmat, mag);
467 
468  /* normalize measured ltp in 2D (x,y) */
469  struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
470  int32_vect2_normalize(&measured_ltp_2d, INT32_MAG_FRAC);
471 
472  /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */
473  struct Int32Vect3 residual_ltp = {
474  0,
475  0,
476  (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x) / (1 << 5)
477  };
478 
479 
480  struct Int32Vect3 residual_body;
481  int32_rmat_vmult(&residual_body, &ltp_to_body_rmat, &residual_ltp);
482 
483  /* Complementary filter proportionnal gain.
484  * Kp = 2 * mag_zeta * mag_omega
485  * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
486  * with ahrs_icq.mag_cnt beeing the number of propagations since last update
487  *
488  * residual_body FRAC = residual_ltp FRAC = 17
489  * rate_correction FRAC: RATE_FRAC = 12
490  * FRAC conversion: 2^12 / 2^17 = 1/32
491  *
492  * inv_rate_gain = 1 / Kp / FRAC_conversion
493  * inv_rate_gain = 32 / Kp
494  */
495  int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
496 
497  ahrs_icq.rate_correction.p += (residual_body.x / inv_rate_gain);
498  ahrs_icq.rate_correction.q += (residual_body.y / inv_rate_gain);
499  ahrs_icq.rate_correction.r += (residual_body.z / inv_rate_gain);
500 
501  /* Complementary filter integral gain
502  * Correct the gyro bias.
503  * Ki = omega^2 * dt
504  *
505  * residual_body FRAC = residual_ltp FRAC = 17
506  * high_rez_bias FRAC: RATE_FRAC+28 = 40
507  * FRAC conversion: 2^40 / 2^17 = 2^23
508  *
509  * bias_gain = Ki * FRAC_conversion = Ki * 2^23
510  */
511  int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 23));
512 
513  ahrs_icq.high_rez_bias.p -= (residual_body.x * bias_gain);
514  ahrs_icq.high_rez_bias.q -= (residual_body.y * bias_gain);
515  ahrs_icq.high_rez_bias.r -= (residual_body.z * bias_gain);
516 
518 
519 }
520 
521 void ahrs_icq_update_gps(struct GpsState *gps_s __attribute__((unused)))
522 {
523 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
524  if (gps_s->fix >= GPS_FIX_3D) {
525  ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps_s->speed_3d / 100.);
527  } else {
529  }
530 #endif
531 
532 #if AHRS_USE_GPS_HEADING && USE_GPS
533  // got a 3d fix, ground speed > AHRS_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
534  // and course accuracy is better than 10deg
535  static const uint16_t gps_min_speed = AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100;
536  static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
537  if (gps_s->fix >= GPS_FIX_3D &&
538  gps_s->gspeed >= gps_min_speed &&
539  gps_s->cacc <= max_cacc) {
540 
541  // gps_s->course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
542  int32_t course = gps_s->course * ((1 << INT32_ANGLE_FRAC) / 1e7);
543 
544  /* the assumption here is that there is no side-slip, so heading=course */
545 
548  } else {
549  /* hard reset the heading if this is the first measurement */
551  }
552  }
553 #endif
554 }
555 
556 
558 {
559 
561 
562  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
563  // we only consider x and y
564  struct Int32RMat ltp_to_body_rmat;
565  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
566  struct Int32Vect2 expected_ltp = {
567  RMAT_ELMT(ltp_to_body_rmat, 0, 0),
568  RMAT_ELMT(ltp_to_body_rmat, 0, 1)
569  };
570 
571  int32_t heading_x, heading_y;
572  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
573  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
574 
575  // expected_heading cross measured_heading ??
576  struct Int32Vect3 residual_ltp = {
577  0,
578  0,
579  (expected_ltp.x * heading_y - expected_ltp.y * heading_x) / (1 << INT32_ANGLE_FRAC)
580  };
581 
582  struct Int32Vect3 residual_body;
583  int32_rmat_vmult(&residual_body, &ltp_to_body_rmat, &residual_ltp);
584 
585  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
586  // rate_correction FRAC = RATE_FRAC = 12
587  // 2^12 / 2^28 * 4.0 = 1/2^14
588  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
589  ahrs_icq.rate_correction.p += residual_body.x / 4;
590  ahrs_icq.rate_correction.q += residual_body.y / 4;
591  ahrs_icq.rate_correction.r += residual_body.z / 4;
592 
593 
594  /* crude attempt to only update bias if deviation is small
595  * e.g. needed when you only have gps providing heading
596  * and the inital heading is totally different from
597  * the gps course information you get once you have a gps fix.
598  * Otherwise the bias will be falsely "corrected".
599  */
600  int32_t sin_max_angle_deviation;
601  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(AHRS_BIAS_UPDATE_HEADING_THRESHOLD)));
602  if (ABS(residual_ltp.z) < sin_max_angle_deviation) {
603  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
604  // high_rez_bias = RATE_FRAC+28 = 40
605  // 2^40 / 2^28 * 2.5e-4 = 1
606  ahrs_icq.high_rez_bias.p -= residual_body.x * (1 << INT32_ANGLE_FRAC);
607  ahrs_icq.high_rez_bias.q -= residual_body.y * (1 << INT32_ANGLE_FRAC);
608  ahrs_icq.high_rez_bias.r -= residual_body.z * (1 << INT32_ANGLE_FRAC);
609 
611  }
612 }
613 
615 {
616  /* quaternion representing only the heading rotation from ltp to body */
617  struct Int32Quat q_h_new;
618  q_h_new.qx = 0;
619  q_h_new.qy = 0;
620  PPRZ_ITRIG_SIN(q_h_new.qz, heading / 2);
621  PPRZ_ITRIG_COS(q_h_new.qi, heading / 2);
622 
623  /* quaternion representing current heading only */
624  struct Int32Quat q_h;
626  q_h.qx = 0;
627  q_h.qy = 0;
628  int32_quat_normalize(&q_h);
629 
630  /* quaternion representing rotation from current to new heading */
631  struct Int32Quat q_c;
632  int32_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
633 
634  /* correct current heading in body frame */
635  struct Int32Quat q;
638 
639  ahrs_icq.heading_aligned = true;
640 }
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
#define AHRS_MAG_OMEGA
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
#define ACC_FROM_CROSS_FRAC
#define AHRS_MAG_ZETA
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD
don't update gyro bias if heading deviation is above this threshold in degrees
void ahrs_icq_init(void)
void ahrs_icq_update_gps(struct GpsState *gps_s)
void ahrs_icq_update_heading(int32_t heading)
Update yaw based on a heading measurement.
static void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
void ahrs_icq_realign_heading(int32_t heading)
Hard reset yaw to a heading.
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
#define FIR_FILTER_SIZE
void ahrs_icq_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
#define AHRS_ACCEL_OMEGA
void ahrs_icq_set_mag_gains(void)
update pre-computed kp and ki gains from mag_omega and mag_zeta
static void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
#define AHRS_ACCEL_ZETA
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
#define COMPUTATION_FRAC
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED
Minimum speed in m/s for heading update via GPS.
Quaternion complementary filter (fixed-point).
uint16_t accel_cnt
number of propagations since last accel update
struct Int32Rates gyro_bias
@ AHRS_ICQ_UNINIT
@ AHRS_ICQ_RUNNING
uint16_t mag_cnt
number of propagations since last mag update
struct Int32Vect3 mag_h
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer.
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
struct Int32Rates rate_correction
float mag_zeta
filter damping for correcting the gyro bias from magnetometer.
struct Int64Rates high_rez_bias
float accel_omega
filter cut-off frequency for correcting the attitude from accels.
struct Int32Rates body_rate
struct Int32Quat ltp_to_body_quat
struct Int64Quat high_rez_quat
enum AhrsICQStatus status
status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
bool correct_gravity
enable gravity vector correction by removing centrifugal acceleration
float accel_zeta
filter damping for correcting the gyro-bias from accels.
Ahrs implementation specifc values.
Utility functions for fixed point AHRS implementations.
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
static int16_t course[3]
Definition: airspeed_uADC.c:58
static uint16_t c2
Definition: baro_MS5534A.c:203
uint8_t last_wp UNUSED
Device independent GPS code (interface)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
data structure for GPS information
Definition: gps.h:90
#define FLOAT_VECT3_NORM(_v)
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
Definition: pprz_algebra.h:254
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:386
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
#define INT_RATES_ZERO(_e)
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
#define INT32_MAG_FRAC
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
#define INT_RATES_LSHIFT(_o, _i, _r)
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
normalize 2D vector inplace
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
#define INT_RATES_RSHIFT(_o, _i, _r)
#define MAG_BFP_OF_REAL(_af)
#define INT32_VECT3_RSHIFT(_o, _i, _r)
void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
in place quaternion first order integration with constant rotational velocity.
#define INT32_ANGLE_FRAC
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define TRIG_BFP_OF_REAL(_tf)
#define SPEED_BFP_OF_REAL(_af)
#define INT32_ANGLE_NORMALIZE(_a)
Rotation quaternion.
rotation matrix
angular rates
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Paparazzi fixed point algebra.
Paparazzi fixed point trig functions.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
float heading
Definition: wedgebug.c:258