Paparazzi UAS  v7.0_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 #ifdef AHRS_FLOATING_HEADING
47 PRINT_CONFIG_MSG("No heading feedback in AHRS: FLOATING HEADING")
48 #endif
49 
50 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
51 #warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
52 #endif
53 
54 #if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING &&!AHRS_FLOATING_HEADING
55 #warning "Please use either USE_MAGNETOMETER, AHRS_USE_GPS_HEADING or accept AHRS_FLOATING_HEADING."
56 #endif
57 
58 #if AHRS_USE_GPS_HEADING && !USE_GPS
59 #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
60 #endif
61 
62 /*
63  * default gains for correcting attitude and bias from accel/mag
64  */
65 #ifndef AHRS_ACCEL_OMEGA
66 #define AHRS_ACCEL_OMEGA 0.063
67 #endif
68 #ifndef AHRS_ACCEL_ZETA
69 #define AHRS_ACCEL_ZETA 0.9
70 #endif
71 PRINT_CONFIG_VAR(AHRS_ACCEL_OMEGA)
72 PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA)
73 
74 
75 #ifndef AHRS_MAG_OMEGA
76 #define AHRS_MAG_OMEGA 0.04
77 #endif
78 #ifndef AHRS_MAG_ZETA
79 #define AHRS_MAG_ZETA 0.9
80 #endif
81 #if USE_MAGNETOMETER
82 PRINT_CONFIG_VAR(AHRS_MAG_OMEGA)
83 PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
84 #endif
85 
87 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
88 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
89 #endif
90 
92 #ifndef AHRS_BIAS_UPDATE_HEADING_THRESHOLD
93 #define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0
94 #endif
95 
99 #ifndef AHRS_HEADING_UPDATE_GPS_MIN_SPEED
100 #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
101 #endif
102 
104 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
105 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
106 #define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
107 #endif
108 
109 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
110 #define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
111 #endif
112 #endif
113 
115 
116 static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt);
117 static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt);
118 
119 void ahrs_icq_init(void)
120 {
121 
123  ahrs_icq.is_aligned = false;
124 
126  ahrs_icq.heading_aligned = false;
127 
128  /* init ltp_to_body quaternion as zero/identity rotation */
131 
135 
136  /* set default filter cut-off frequency and damping */
143 
144  /* set default gravity heuristic */
146 
147 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
148  ahrs_icq.correct_gravity = true;
149 #else
150  ahrs_icq.correct_gravity = false;
151 #endif
152 
154  MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
155 
156  ahrs_icq.accel_cnt = 0;
157  ahrs_icq.mag_cnt = 0;
158 }
159 
160 
161 bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
162  struct Int32Vect3 *lp_mag)
163 {
164 
165 #if USE_MAGNETOMETER
166  /* Compute an initial orientation from accel and mag directly as quaternion */
168  lp_accel, lp_mag);
169  ahrs_icq.heading_aligned = true;
170 #else
171  /* Compute an initial orientation from accel and just set heading to zero */
173  ahrs_icq.heading_aligned = false;
174  // supress unused arg warning
175  lp_mag = lp_mag;
176 #endif
177 
178  /* Use low passed gyro value as initial bias */
179  RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro);
180  RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
182 
184  ahrs_icq.is_aligned = true;
185 
186  return true;
187 }
188 
189 
190 void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
191 {
192  int32_t freq = (int32_t)(1. / dt);
193 
194  /* unbias gyro */
195  struct Int32Rates omega;
196  RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);
197 
198  /* low pass rate */
199 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
200  RATES_SMUL(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
201  RATES_ADD(ahrs_icq.body_rate, omega);
202  RATES_SDIV(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
203 #else
204  RATES_COPY(ahrs_icq.body_rate, omega);
205 #endif
206 
207  /* add correction */
209  /* and zeros it */
211 
212  /* integrate quaternion */
214  &omega, freq);
216 
217  // increase accel and mag propagation counters
219  ahrs_icq.mag_cnt++;
220 }
221 
222 
224 {
225  /* Complementary filter proportionnal gain (without frequency correction)
226  * Kp = 2 * omega * zeta
227  *
228  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
229  * accel_inv_kp = 4096 * 9.81 / Kp
230  */
231  ahrs_icq.accel_inv_kp = 4096 * 9.81 /
233 
234  /* Complementary filter integral gain
235  * Ki = omega^2
236  *
237  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
238  * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki
239  * accel_inv_ki = 9.81 / 2048 / Ki
240  */
242 }
243 
244 void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
245 {
246  // check if we had at least one propagation since last update
247  if (ahrs_icq.accel_cnt == 0) {
248  return;
249  }
250 
251  // c2 = ltp z-axis in imu-frame
252  struct Int32RMat ltp_to_body_rmat;
253  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
254  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_body_rmat, 0, 2),
255  RMAT_ELMT(ltp_to_body_rmat, 1, 2),
256  RMAT_ELMT(ltp_to_body_rmat, 2, 2)
257  };
258  struct Int32Vect3 residual;
259 
260  struct Int32Vect3 pseudo_gravity_measurement;
261 
263  /*
264  * centrifugal acceleration in body frame
265  * a_c_body = omega x (omega x r)
266  * (omega x r) = tangential velocity in body frame
267  * a_c_body = omega x vel_tangential_body
268  * assumption: tangential velocity only along body x-axis (or negative z-axis)
269  */
270 
271  // FIXME: check overflows !
272 #define COMPUTATION_FRAC 16
273 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
274 
275  const struct Int32Vect3 vel_tangential_body =
276 #if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
277  /* AHRS_GRAVITY_UPDATE_COORDINATED_TURN assumes the GPS speed is in the X axis direction.
278  * Quadshot, DelftaCopter and other hybrids can have the GPS speed in the negative Z direction
279  */
280  {0, 0, -(ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC)};
281 #else
282  /* assume tangential velocity along body x-axis */
284 #endif
285  struct Int32Vect3 acc_c_body;
286  VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_icq.body_rate, vel_tangential_body);
287  INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
288 
289  /* centrifucal acceleration subtract it from imu measurement to get a corrected measurement
290  * of the gravity vector */
291  VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_body);
292  } else {
293  VECT3_COPY(pseudo_gravity_measurement, *accel);
294  }
295 
296  /* compute the residual of the pseudo gravity vector in imu frame */
297  VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
298 
299 
300  /* FIR filtered pseudo_gravity_measurement */
301 #define FIR_FILTER_SIZE 8
302  static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
303  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
304  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
305  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
306 
307 
309  /* heuristic on acceleration (gravity estimate) norm */
310  /* Factor how strongly to change the weight.
311  * e.g. for gravity_heuristic_factor 30:
312  * <0.66G = 0, 1G = 1.0, >1.33G = 0
313  */
314 
315  struct FloatVect3 g_meas_f;
316  ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
317  const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f) / 9.81;
318  ahrs_icq.weight = 1.0 - ahrs_icq.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10;
319  Bound(ahrs_icq.weight, 0.15, 1.0);
320  } else {
321  ahrs_icq.weight = 1.0;
322  }
323 
324  /* Complementary filter proportional gain.
325  * Kp = 2 * zeta * omega
326  * final Kp with frequency correction = Kp * ahrs_icq.accel_cnt
327  * with ahrs_icq.accel_cnt beeing the number of propagations since last update
328  *
329  * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
330  * rate_correction FRAC: RATE_FRAC = 12
331  * FRAC_conversion: 2^12 / 2^24 = 1 / 4096
332  * cross_product_gain : 9.81 m/s2
333  *
334  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
335  * accel_inv_kp = 4096 * 9.81 / Kp
336  *
337  * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain)
338  * inv_rate_scale = 1 / Kp / weight
339  * inv_rate_scale = accel_inv_kp / accel_cnt / weight
340  */
342  / ahrs_icq.weight);
343  Bound(inv_rate_scale, 8192, 4194304);
344 
345  ahrs_icq.rate_correction.p -= residual.x / inv_rate_scale;
346  ahrs_icq.rate_correction.q -= residual.y / inv_rate_scale;
347  ahrs_icq.rate_correction.r -= residual.z / inv_rate_scale;
348 
349  // reset accel propagation counter
350  ahrs_icq.accel_cnt = 0;
351 
352  /* Complementary filter integral gain
353  * Correct the gyro bias.
354  * Ki = omega^2 * dt
355  *
356  * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
357  * high_rez_bias = RATE_FRAC+28 = 40
358  * FRAC_conversion: 2^40 / 2^24 = 2^16
359  * cross_product_gain : 9.81 m/s2
360  *
361  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
362  * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki
363  *
364  * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain)
365  * inv_bias_gain = accel_inv_ki / weight^2
366  */
367 
368  int32_t inv_bias_gain = (int32_t)(ahrs_icq.accel_inv_ki /
369  (dt * ahrs_icq.weight * ahrs_icq.weight));
370  Bound(inv_bias_gain, 8, 65536)
371  ahrs_icq.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
372  ahrs_icq.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
373  ahrs_icq.high_rez_bias.r += (residual.z / inv_bias_gain) << 5;
374 
376 
377 }
378 
379 
380 void ahrs_icq_update_mag(struct Int32Vect3 *mag __attribute__((unused)), float dt __attribute__((unused)))
381 {
382 #if USE_MAGNETOMETER
383  // check if we had at least one propagation since last update
384  if (ahrs_icq.mag_cnt == 0) {
385  return;
386  }
387 #if AHRS_MAG_UPDATE_ALL_AXES
388  ahrs_icq_update_mag_full(mag, dt);
389 #else
390  ahrs_icq_update_mag_2d(mag, dt);
391 #endif
392  // reset mag propagation counter
393  ahrs_icq.mag_cnt = 0;
394 #endif
395 }
396 
398 {
399  /* Complementary filter proportionnal gain = 2*omega*zeta */
401  /* Complementary filter integral gain = omega^2 */
403 }
404 
405 
406 static inline void ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
407 {
408 
409  struct Int32RMat ltp_to_body_rmat;
410  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
411 
412  struct Int32Vect3 expected_imu;
413  int32_rmat_vmult(&expected_imu, &ltp_to_body_rmat, &ahrs_icq.mag_h);
414 
415  struct Int32Vect3 residual;
416  VECT3_CROSS_PRODUCT(residual, *mag, expected_imu);
417 
418  /* Complementary filter proportionnal gain.
419  * Kp = 2 * mag_zeta * mag_omega
420  * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
421  * with ahrs_icq.mag_cnt beeing the number of propagations since last update
422  *
423  * residual FRAC: 2 * MAG_FRAC = 22
424  * rate_correction FRAC: RATE_FRAC = 12
425  * FRAC conversion: 2^12 / 2^22 = 1/1024
426  *
427  * inv_rate_gain = 1 / Kp / FRAC_conversion
428  * inv_rate_gain = 1024 / Kp
429  */
430 
431  const int32_t inv_rate_gain = (int32_t)(1024.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
432 
433  ahrs_icq.rate_correction.p += residual.x / inv_rate_gain;
434  ahrs_icq.rate_correction.q += residual.y / inv_rate_gain;
435  ahrs_icq.rate_correction.r += residual.z / inv_rate_gain;
436 
437  /* Complementary filter integral gain
438  * Correct the gyro bias.
439  * Ki = omega^2 * dt
440  *
441  * residual FRAC: 2* MAG_FRAC = 22
442  * high_rez_bias FRAC: RATE_FRAC+28 = 40
443  * FRAC conversion: 2^40 / 2^22 = 2^18
444  *
445  * bias_gain = Ki * FRAC_conversion = Ki * 2^18
446  */
447  const int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 18));
448 
449  ahrs_icq.high_rez_bias.p -= residual.x * bias_gain;
450  ahrs_icq.high_rez_bias.q -= residual.y * bias_gain;
451  ahrs_icq.high_rez_bias.r -= residual.z * bias_gain;
452 
453 
455 
456 }
457 
458 
459 static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
460 {
461 
462  struct Int32Vect2 expected_ltp = {ahrs_icq.mag_h.x, ahrs_icq.mag_h.y};
463  /* normalize expected ltp in 2D (x,y) */
464  int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC);
465 
466  struct Int32RMat ltp_to_body_rmat;
467  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
468 
469  struct Int32Vect3 measured_ltp;
470  int32_rmat_transp_vmult(&measured_ltp, &ltp_to_body_rmat, mag);
471 
472  /* normalize measured ltp in 2D (x,y) */
473  struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
474  int32_vect2_normalize(&measured_ltp_2d, INT32_MAG_FRAC);
475 
476  /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */
477  struct Int32Vect3 residual_ltp = {
478  0,
479  0,
480  (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x) / (1 << 5)
481  };
482 
483 
484  struct Int32Vect3 residual_body;
485  int32_rmat_vmult(&residual_body, &ltp_to_body_rmat, &residual_ltp);
486 
487  /* Complementary filter proportionnal gain.
488  * Kp = 2 * mag_zeta * mag_omega
489  * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
490  * with ahrs_icq.mag_cnt beeing the number of propagations since last update
491  *
492  * residual_body FRAC = residual_ltp FRAC = 17
493  * rate_correction FRAC: RATE_FRAC = 12
494  * FRAC conversion: 2^12 / 2^17 = 1/32
495  *
496  * inv_rate_gain = 1 / Kp / FRAC_conversion
497  * inv_rate_gain = 32 / Kp
498  */
499  int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
500 
501  ahrs_icq.rate_correction.p += (residual_body.x / inv_rate_gain);
502  ahrs_icq.rate_correction.q += (residual_body.y / inv_rate_gain);
503  ahrs_icq.rate_correction.r += (residual_body.z / inv_rate_gain);
504 
505  /* Complementary filter integral gain
506  * Correct the gyro bias.
507  * Ki = omega^2 * dt
508  *
509  * residual_body FRAC = residual_ltp FRAC = 17
510  * high_rez_bias FRAC: RATE_FRAC+28 = 40
511  * FRAC conversion: 2^40 / 2^17 = 2^23
512  *
513  * bias_gain = Ki * FRAC_conversion = Ki * 2^23
514  */
515  int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 23));
516 
517  ahrs_icq.high_rez_bias.p -= (residual_body.x * bias_gain);
518  ahrs_icq.high_rez_bias.q -= (residual_body.y * bias_gain);
519  ahrs_icq.high_rez_bias.r -= (residual_body.z * bias_gain);
520 
522 
523 }
524 
525 void ahrs_icq_update_gps(struct GpsState *gps_s __attribute__((unused)))
526 {
527 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
528  if (gps_s->fix >= GPS_FIX_3D) {
529  ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps_s->speed_3d / 100.);
531  } else {
533  }
534 #endif
535 
536 #if AHRS_USE_GPS_HEADING && USE_GPS
537  // got a 3d fix, ground speed > AHRS_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
538  // and course accuracy is better than 10deg
539  static const uint16_t gps_min_speed = AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100;
540  static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
541  if (gps_s->fix >= GPS_FIX_3D &&
542  gps_s->gspeed >= gps_min_speed &&
543  gps_s->cacc <= max_cacc) {
544 
545  // gps_s->course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
546  int32_t course = gps_s->course * ((1 << INT32_ANGLE_FRAC) / 1e7);
547 
548  /* the assumption here is that there is no side-slip, so heading=course */
549 
552  } else {
553  /* hard reset the heading if this is the first measurement */
555  }
556  }
557 #endif
558 }
559 
560 
562 {
563 
565 
566  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
567  // we only consider x and y
568  struct Int32RMat ltp_to_body_rmat;
569  int32_rmat_of_quat(&ltp_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
570  struct Int32Vect2 expected_ltp = {
571  RMAT_ELMT(ltp_to_body_rmat, 0, 0),
572  RMAT_ELMT(ltp_to_body_rmat, 0, 1)
573  };
574 
575  int32_t heading_x, heading_y;
576  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
577  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
578 
579  // expected_heading cross measured_heading ??
580  struct Int32Vect3 residual_ltp = {
581  0,
582  0,
583  (expected_ltp.x * heading_y - expected_ltp.y * heading_x) / (1 << INT32_ANGLE_FRAC)
584  };
585 
586  struct Int32Vect3 residual_body;
587  int32_rmat_vmult(&residual_body, &ltp_to_body_rmat, &residual_ltp);
588 
589  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
590  // rate_correction FRAC = RATE_FRAC = 12
591  // 2^12 / 2^28 * 4.0 = 1/2^14
592  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
593  ahrs_icq.rate_correction.p += residual_body.x / 4;
594  ahrs_icq.rate_correction.q += residual_body.y / 4;
595  ahrs_icq.rate_correction.r += residual_body.z / 4;
596 
597 
598  /* crude attempt to only update bias if deviation is small
599  * e.g. needed when you only have gps providing heading
600  * and the inital heading is totally different from
601  * the gps course information you get once you have a gps fix.
602  * Otherwise the bias will be falsely "corrected".
603  */
604  int32_t sin_max_angle_deviation;
605  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(AHRS_BIAS_UPDATE_HEADING_THRESHOLD)));
606  if (ABS(residual_ltp.z) < sin_max_angle_deviation) {
607  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
608  // high_rez_bias = RATE_FRAC+28 = 40
609  // 2^40 / 2^28 * 2.5e-4 = 1
610  ahrs_icq.high_rez_bias.p -= residual_body.x * (1 << INT32_ANGLE_FRAC);
611  ahrs_icq.high_rez_bias.q -= residual_body.y * (1 << INT32_ANGLE_FRAC);
612  ahrs_icq.high_rez_bias.r -= residual_body.z * (1 << INT32_ANGLE_FRAC);
613 
615  }
616 }
617 
619 {
620  /* quaternion representing only the heading rotation from ltp to body */
621  struct Int32Quat q_h_new;
622  q_h_new.qx = 0;
623  q_h_new.qy = 0;
624  PPRZ_ITRIG_SIN(q_h_new.qz, heading / 2);
625  PPRZ_ITRIG_COS(q_h_new.qi, heading / 2);
626 
627  /* quaternion representing current heading only */
628  struct Int32Quat q_h;
630  q_h.qx = 0;
631  q_h.qy = 0;
632  int32_quat_normalize(&q_h);
633 
634  /* quaternion representing rotation from current to new heading */
635  struct Int32Quat q_c;
636  int32_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
637 
638  /* correct current heading in body frame */
639  struct Int32Quat q;
642 
643  ahrs_icq.heading_aligned = true;
644 }
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:85
#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