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