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