Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
36 
37 #include "state.h"
38 
39 #include "subsystems/imu.h"
40 #if USE_GPS
41 #include "subsystems/gps.h"
42 #endif
43 #include "math/pprz_trig_int.h"
44 #include "math/pprz_algebra_int.h"
45 
46 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
47 PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
48 #endif
49 
50 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
51 #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."
52 #endif
53 
54 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
55 #warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
56 #endif
57 
58 #if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING
59 #warning "Please use either USE_MAGNETOMETER or AHRS_USE_GPS_HEADING."
60 #endif
61 
62 #if AHRS_USE_GPS_HEADING && !USE_GPS
63 #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
64 #endif
65 
66 #ifndef AHRS_PROPAGATE_FREQUENCY
67 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
68 #endif
70 #if AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY
71 #warning PERIODIC_FREQUENCY should be >= AHRS_PROPAGATE_FREQUENCY
72 #endif
73 
74 #ifndef AHRS_CORRECT_FREQUENCY
75 #define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY
76 #endif
78 #if AHRS_CORRECT_FREQUENCY > PERIODIC_FREQUENCY
79 #warning PERIODIC_FREQUENCY should be >= AHRS_CORRECT_FREQUENCY
80 #endif
81 
82 #ifndef AHRS_MAG_CORRECT_FREQUENCY
83 #define AHRS_MAG_CORRECT_FREQUENCY 50
84 #endif
85 #if USE_MAGNETOMETER
87 #endif
88 
89 /*
90  * default gains for correcting attitude and bias from accel/mag
91  */
92 #ifndef AHRS_ACCEL_OMEGA
93 #define AHRS_ACCEL_OMEGA 0.063
94 #endif
95 #ifndef AHRS_ACCEL_ZETA
96 #define AHRS_ACCEL_ZETA 0.9
97 #endif
100 
101 
102 #ifndef AHRS_MAG_OMEGA
103 #define AHRS_MAG_OMEGA 0.04
104 #endif
105 #ifndef AHRS_MAG_ZETA
106 #define AHRS_MAG_ZETA 0.9
107 #endif
108 #if USE_MAGNETOMETER
111 #endif
112 
114 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
115 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
116 #endif
117 
119 #ifndef AHRS_BIAS_UPDATE_HEADING_THRESHOLD
120 #define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0
121 #endif
122 
126 #ifndef AHRS_HEADING_UPDATE_GPS_MIN_SPEED
127 #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
128 #endif
129 
130 #ifdef AHRS_UPDATE_FW_ESTIMATOR
131 // remotely settable
132 #ifndef INS_ROLL_NEUTRAL_DEFAULT
133 #define INS_ROLL_NEUTRAL_DEFAULT 0
134 #endif
135 #ifndef INS_PITCH_NEUTRAL_DEFAULT
136 #define INS_PITCH_NEUTRAL_DEFAULT 0
137 #endif
140 #endif
141 
143 
144 static inline void set_body_state_from_quat(void);
145 static inline void UNUSED ahrs_update_mag_full(void);
146 static inline void ahrs_update_mag_2d(void);
147 
148 #if PERIODIC_TELEMETRY
150 
151 static void send_quat(void) {
152  struct Int32Quat* quat = stateGetNedToBodyQuat_i();
153  DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice,
154  &ahrs_impl.weight,
159  &(quat->qi),
160  &(quat->qx),
161  &(quat->qy),
162  &(quat->qz));
163 }
164 
165 static void send_euler(void) {
166  struct Int32Eulers ltp_to_imu_euler;
168  struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
169  DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
170  &ltp_to_imu_euler.phi,
171  &ltp_to_imu_euler.theta,
172  &ltp_to_imu_euler.psi,
173  &(eulers->phi),
174  &(eulers->theta),
175  &(eulers->psi));
176 }
177 
178 static void send_bias(void) {
179  DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice,
181 }
182 
183 static void send_geo_mag(void) {
184  struct FloatVect3 h_float;
185  h_float.x = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.x);
186  h_float.y = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.y);
187  h_float.z = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.z);
188  DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice,
189  &h_float.x, &h_float.y, &h_float.z);
190 }
191 #endif
192 
193 void ahrs_init(void) {
194 
198 
199  /* set ltp_to_imu so that body is zero */
201  sizeof(struct Int32Quat));
203 
207 
208  /* set default filter cut-off frequency and damping */
215 
216  /* set default gravity heuristic */
218 
219 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
221 #else
223 #endif
224 
226  MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
227 
228 #if PERIODIC_TELEMETRY
229  register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
230  register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
231  register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
232  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
233 #endif
234 
235 }
236 
237 
238 void ahrs_align(void) {
239 
240 #if USE_MAGNETOMETER
241  /* Compute an initial orientation from accel and mag directly as quaternion */
245 #else
246  /* Compute an initial orientation from accel and just set heading to zero */
249 #endif
250 
252 
253  /* Use low passed gyro value as initial bias */
257 
259 }
260 
261 
262 void ahrs_propagate(void) {
263 
264  /* unbias gyro */
265  struct Int32Rates omega;
267 
268  /* low pass rate */
269 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
271  RATES_ADD(ahrs_impl.imu_rate, omega);
273 #else
274  RATES_COPY(ahrs_impl.imu_rate, omega);
275 #endif
276 
277  /* add correction */
279  /* and zeros it */
281 
282  /* integrate quaternion */
284  omega, AHRS_PROPAGATE_FREQUENCY);
286 
288 
289 }
290 
291 
293  /* Complementary filter proportionnal gain
294  * Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
295  *
296  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
297  * accel_inv_kp = 4096 * 9.81 / Kp
298  */
299  ahrs_impl.accel_inv_kp = 4096 * 9.81 /
302 
303  /* Complementary filter integral gain
304  * Ki = omega^2 / AHRS_CORRECT_FREQUENCY
305  *
306  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
307  * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki
308  * accel_inv_ki = 9.81 / 2048 / Ki
309  */
312 }
313 
314 void ahrs_update_accel(void) {
315 
316  // c2 = ltp z-axis in imu-frame
317  struct Int32RMat ltp_to_imu_rmat;
318  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
319  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2),
320  RMAT_ELMT(ltp_to_imu_rmat, 1,2),
321  RMAT_ELMT(ltp_to_imu_rmat, 2,2)};
322  struct Int32Vect3 residual;
323 
324  struct Int32Vect3 pseudo_gravity_measurement;
325 
327  /*
328  * centrifugal acceleration in body frame
329  * a_c_body = omega x (omega x r)
330  * (omega x r) = tangential velocity in body frame
331  * a_c_body = omega x vel_tangential_body
332  * assumption: tangential velocity only along body x-axis
333  */
334 
335  // FIXME: check overflows !
336 #define COMPUTATION_FRAC 16
337 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
338 
339  const struct Int32Vect3 vel_tangential_body =
341  struct Int32Vect3 acc_c_body;
342  VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body);
343  INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
344 
345  /* convert centrifucal acceleration from body to imu frame */
346  struct Int32Vect3 acc_c_imu;
347  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
348  INT32_RMAT_VMULT(acc_c_imu, *body_to_imu_rmat, acc_c_body);
349 
350  /* and subtract it from imu measurement to get a corrected measurement
351  * of the gravity vector */
352  INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
353  } else {
354  VECT3_COPY(pseudo_gravity_measurement, imu.accel);
355  }
356 
357  /* compute the residual of the pseudo gravity vector in imu frame */
358  INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
359 
360 
361  /* FIR filtered pseudo_gravity_measurement */
362  #define FIR_FILTER_SIZE 8
363  static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
364  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
365  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
366  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
367 
368 
370  /* heuristic on acceleration (gravity estimate) norm */
371  /* Factor how strongly to change the weight.
372  * e.g. for gravity_heuristic_factor 30:
373  * <0.66G = 0, 1G = 1.0, >1.33G = 0
374  */
375 
376  struct FloatVect3 g_meas_f;
377  ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
378  const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81;
379  ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10;
380  Bound(ahrs_impl.weight, 0.15, 1.0);
381  }
382  else {
383  ahrs_impl.weight = 1.0;
384  }
385 
386  /* Complementary filter proportional gain.
387  * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
388  *
389  * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
390  * rate_correction FRAC: RATE_FRAC = 12
391  * FRAC_conversion: 2^12 / 2^24 = 1 / 4096
392  * cross_product_gain : 9.81 m/s2
393  *
394  * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
395  * accel_inv_kp = 4096 * 9.81 / Kp
396  *
397  * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain)
398  * inv_rate_scale = inv_kp / weight
399  */
400  int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight);
401  Bound(inv_rate_scale, 8192, 4194304);
402 
403  ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale;
404  ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale;
405  ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale;
406 
407 
408  /* Complementary filter integral gain
409  * Correct the gyro bias.
410  * Ki = omega^2 / AHRS_CORRECT_FREQUENCY
411  *
412  * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
413  * high_rez_bias = RATE_FRAC+28 = 40
414  * FRAC_conversion: 2^40 / 2^24 = 2^16
415  * cross_product_gain : 9.81 m/s2
416  *
417  * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
418  * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki
419  *
420  * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain)
421  * inv_bias_gain = accel_inv_ki / weight^2
422  */
423 
424  int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki /
426  Bound(inv_bias_gain, 8, 65536)
427  ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
428  ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
429  ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5;
430 
432 
433 }
434 
435 
436 void ahrs_update_mag(void) {
437 #if AHRS_MAG_UPDATE_ALL_AXES
439 #else
441 #endif
442 }
443 
444 void ahrs_set_mag_gains(void) {
445  /* Complementary filter proportionnal gain = 2*omega*zeta */
448  /* Complementary filter integral gain = omega^2 */
451 }
452 
453 
454 static inline void ahrs_update_mag_full(void) {
455 
456  struct Int32RMat ltp_to_imu_rmat;
457  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
458 
459  struct Int32Vect3 expected_imu;
460  INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, ahrs_impl.mag_h);
461 
462  struct Int32Vect3 residual;
463  INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
464 
465  /* Complementary filter proportionnal gain.
466  * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
467  *
468  * residual FRAC: 2 * MAG_FRAC = 22
469  * rate_correction FRAC: RATE_FRAC = 12
470  * FRAC conversion: 2^12 / 2^22 = 1/1024
471  *
472  * inv_rate_gain = 1 / Kp / FRAC_conversion
473  * inv_rate_gain = 1024 / Kp
474  */
475 
476  const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp);
477 
478  ahrs_impl.rate_correction.p += residual.x / inv_rate_gain;
479  ahrs_impl.rate_correction.q += residual.y / inv_rate_gain;
480  ahrs_impl.rate_correction.r += residual.z / inv_rate_gain;
481 
482  /* Complementary filter integral gain
483  * Correct the gyro bias.
484  * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
485  *
486  * residual FRAC: 2* MAG_FRAC = 22
487  * high_rez_bias FRAC: RATE_FRAC+28 = 40
488  * FRAC conversion: 2^40 / 2^22 = 2^18
489  *
490  * bias_gain = Ki * FRAC_conversion = Ki * 2^18
491  */
492  const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18));
493 
494  ahrs_impl.high_rez_bias.p -= residual.x * bias_gain;
495  ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
496  ahrs_impl.high_rez_bias.r -= residual.z * bias_gain;
497 
498 
500 
501 }
502 
503 
504 static inline void ahrs_update_mag_2d(void) {
505 
506  struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y};
507  /* normalize expected ltp in 2D (x,y) */
508  INT32_VECT2_NORMALIZE(expected_ltp, INT32_MAG_FRAC);
509 
510  struct Int32RMat ltp_to_imu_rmat;
511  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
512 
513  struct Int32Vect3 measured_ltp;
514  INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag);
515 
516  /* normalize measured ltp in 2D (x,y) */
517  struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
518  INT32_VECT2_NORMALIZE(measured_ltp_2d, INT32_MAG_FRAC);
519 
520  /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */
521  struct Int32Vect3 residual_ltp =
522  { 0,
523  0,
524  (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x)/(1<<5)};
525 
526 
527  struct Int32Vect3 residual_imu;
528  INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp);
529 
530  /* Complementary filter proportionnal gain.
531  * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
532  *
533  * residual_imu FRAC = residual_ltp FRAC = 17
534  * rate_correction FRAC: RATE_FRAC = 12
535  * FRAC conversion: 2^12 / 2^17 = 1/32
536  *
537  * inv_rate_gain = 1 / Kp / FRAC_conversion
538  * inv_rate_gain = 32 / Kp
539  */
540  int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp);
541 
542  ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain);
543  ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain);
544  ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain);
545 
546  /* Complementary filter integral gain
547  * Correct the gyro bias.
548  * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
549  *
550  * residual_imu FRAC = residual_ltp FRAC = 17
551  * high_rez_bias FRAC: RATE_FRAC+28 = 40
552  * FRAC conversion: 2^40 / 2^17 = 2^23
553  *
554  * bias_gain = Ki * FRAC_conversion = Ki * 2^23
555  */
556  int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23));
557 
558  ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain);
559  ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain);
560  ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain);
561 
563 
564 }
565 
566 void ahrs_update_gps(void) {
567 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
568  if (gps.fix == GPS_FIX_3D) {
571  } else {
573  }
574 #endif
575 
576 #if AHRS_USE_GPS_HEADING && USE_GPS
577  // got a 3d fix, ground speed > AHRS_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
578  // and course accuracy is better than 10deg
579  if (gps.fix == GPS_FIX_3D &&
581  gps.cacc <= RadOfDeg(10*1e7)) {
582 
583  // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
584  int32_t course = gps.course * ((1<<INT32_ANGLE_FRAC) / 1e7);
585 
586  /* the assumption here is that there is no side-slip, so heading=course */
587 
589  ahrs_update_heading(course);
590  }
591  else {
592  /* hard reset the heading if this is the first measurement */
593  ahrs_realign_heading(course);
594  }
595  }
596 #endif
597 }
598 
599 
601 
602  INT32_ANGLE_NORMALIZE(heading);
603 
604  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
605  // we only consider x and y
606  struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i();
607  struct Int32Vect2 expected_ltp =
608  { RMAT_ELMT((*ltp_to_body_rmat), 0, 0),
609  RMAT_ELMT((*ltp_to_body_rmat), 0, 1) };
610 
611  int32_t heading_x, heading_y;
612  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
613  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
614 
615  // expected_heading cross measured_heading ??
616  struct Int32Vect3 residual_ltp =
617  { 0,
618  0,
619  (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<<INT32_ANGLE_FRAC)};
620 
621  struct Int32Vect3 residual_imu;
622  struct Int32RMat ltp_to_imu_rmat;
623  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
624  INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp);
625 
626  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
627  // rate_correction FRAC = RATE_FRAC = 12
628  // 2^12 / 2^28 * 4.0 = 1/2^14
629  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
630  ahrs_impl.rate_correction.p += residual_imu.x/4;
631  ahrs_impl.rate_correction.q += residual_imu.y/4;
632  ahrs_impl.rate_correction.r += residual_imu.z/4;
633 
634 
635  /* crude attempt to only update bias if deviation is small
636  * e.g. needed when you only have gps providing heading
637  * and the inital heading is totally different from
638  * the gps course information you get once you have a gps fix.
639  * Otherwise the bias will be falsely "corrected".
640  */
641  int32_t sin_max_angle_deviation;
642  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(AHRS_BIAS_UPDATE_HEADING_THRESHOLD)));
643  if (ABS(residual_ltp.z) < sin_max_angle_deviation)
644  {
645  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
646  // high_rez_bias = RATE_FRAC+28 = 40
647  // 2^40 / 2^28 * 2.5e-4 = 1
648  ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<INT32_ANGLE_FRAC);
649  ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<INT32_ANGLE_FRAC);
650  ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<INT32_ANGLE_FRAC);
651 
653  }
654 }
655 
657 
658  struct Int32Quat ltp_to_body_quat = *stateGetNedToBodyQuat_i();
659 
660  /* quaternion representing only the heading rotation from ltp to body */
661  struct Int32Quat q_h_new;
662  q_h_new.qx = 0;
663  q_h_new.qy = 0;
664  PPRZ_ITRIG_SIN(q_h_new.qz, heading/2);
665  PPRZ_ITRIG_COS(q_h_new.qi, heading/2);
666 
667  /* quaternion representing current heading only */
668  struct Int32Quat q_h;
669  QUAT_COPY(q_h, ltp_to_body_quat);
670  q_h.qx = 0;
671  q_h.qy = 0;
673 
674  /* quaternion representing rotation from current to new heading */
675  struct Int32Quat q_c;
676  INT32_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
677 
678  /* correct current heading in body frame */
679  struct Int32Quat q;
680  INT32_QUAT_COMP_NORM_SHORTEST(q, q_c, ltp_to_body_quat);
681  QUAT_COPY(ltp_to_body_quat, q);
682 
683  /* compute ltp to imu rotations */
684  struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu);
685  INT32_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, ltp_to_body_quat, *body_to_imu_quat);
686 
687  /* Set state */
688  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
689 
691 }
692 
693 
694 /* Rotate angles and rates from imu to body frame and set state */
695 static inline void set_body_state_from_quat(void) {
696  /* Compute LTP to BODY quaternion */
697  struct Int32Quat ltp_to_body_quat;
698  struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu);
699  INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
700  /* Set state */
701 #ifdef AHRS_UPDATE_FW_ESTIMATOR
702  struct Int32Eulers neutrals_to_body_eulers = {
703  ANGLE_BFP_OF_REAL(ins_roll_neutral),
704  ANGLE_BFP_OF_REAL(ins_pitch_neutral),
705  0 };
706  struct Int32Quat neutrals_to_body_quat, ltp_to_neutrals_quat;
707  INT32_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
708  INT32_QUAT_NORMALIZE(neutrals_to_body_quat);
709  INT32_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
710  stateSetNedToBodyQuat_i(&ltp_to_neutrals_quat);
711 #else
712  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
713 #endif
714 
715  /* compute body rates */
716  struct Int32Rates body_rate;
717  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
718  INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate);
719  /* Set state */
720  stateSetBodyRates_i(&body_rate);
721 }
Quaternion complementary filter (fixed-point).
#define AHRS_ACCEL_OMEGA
Interface to align the AHRS via low-passed measurements at startup.
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
int32_t phi
in rad with INT32_ANGLE_FRAC
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static void ahrs_update_mag_2d(void)
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
void ahrs_realign_heading(int32_t heading)
Hard reset yaw to a heading.
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
#define AHRS_ACCEL_ZETA
struct Int32Rates imu_rate
#define ACC_FROM_CROSS_FRAC
#define ANGLE_BFP_OF_REAL(_af)
int32_t p
in rad/s with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
#define AHRS_PROPAGATE_FREQUENCY
Rotation quaternion.
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
void ahrs_init(void)
AHRS initialization.
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
#define INT32_QUAT_NORMALIZE(q)
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f)
in place quaternion first order integration with constant rotational velocity.
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:75
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
void ahrs_align(void)
Aligns the AHRS.
#define INT32_MAG_FRAC
#define INT_RATES_ZERO(_e)
#define AHRS_MAG_CORRECT_FREQUENCY
int32_t theta
in rad with INT32_ANGLE_FRAC
struct Int64Rates high_rez_bias
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:44
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1002
#define MAG_FLOAT_OF_BFP(_ai)
#define INT_RATES_RSHIFT(_o, _i, _r)
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
float accel_zeta
filter damping for correcting the gyro-bias from accels.
#define INT32_VECT2_NORMALIZE(_v, _frac)
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:368
#define FALSE
Definition: imu_chimu.h:141
#define COMPUTATION_FRAC
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:375
static void UNUSED ahrs_update_mag_full(void)
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
#define INT32_QUAT_OF_EULERS(_q, _e)
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
#define FLOAT_VECT3_NORM(_v)
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD
don't update gyro bias if heading deviation is above this threshold in degrees
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1071
float heading
Definition: ahrs_infrared.c:40
#define MAG_BFP_OF_REAL(_af)
void ahrs_update_heading(int32_t heading)
Update yaw based on a heading measurement.
#define INT32_ANGLE_FRAC
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
#define TRIG_BFP_OF_REAL(_tf)
Ahrs implementation specifc values.
float accel_omega
filter cut-off frequency for correcting the attitude from accels.
#define INT32_VECT3_RSHIFT(_o, _i, _r)
Device independent GPS code (interface)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
struct Int32Rates rate_correction
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
Definition: pprz_algebra.h:243
#define INS_PITCH_NEUTRAL_DEFAULT
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
#define INT32_RMAT_VMULT(_vb, _m_a2b, _va)
#define INT32_VECT3_DIFF(_c, _a, _b)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1055
Inertial Measurement Unit interface.
#define INT32_ANGLE_NORMALIZE(_a)
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
angular rates
float ins_pitch_neutral
Definition: ins_arduimu.c:15
signed long int32_t
Definition: types.h:19
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:970
Utility functions for fixed point AHRS implementations.
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
struct AhrsIntCmplQuat ahrs_impl
struct Int32Rates gyro_bias
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:136
#define AHRS_MAG_OMEGA
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
#define INT32_RMAT_OF_QUAT(_rm, _q)
API to get/set the generic vehicle states.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer. ...
int32_t psi
in rad with INT32_ANGLE_FRAC
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
int32_t q
in rad/s with INT32_RATE_FRAC
rotation matrix
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
bool_t correct_gravity
enable gravity vector correction by removing centrifugal acceleration
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct Int32Vect3 mag_h
int32_t r
in rad/s with INT32_RATE_FRAC
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)
struct Int64Quat high_rez_quat
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
void ahrs_set_mag_gains(void)
update pre-computed kp and ki gains from mag_omega and mag_zeta
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
#define INT_RATES_LSHIFT(_o, _i, _r)
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED
Minimum speed in m/s for heading update via GPS.
#define INT32_EULERS_OF_QUAT(_e, _q)
#define SPEED_BFP_OF_REAL(_af)
#define AHRS_MAG_ZETA
struct GpsState gps
global GPS state
Definition: gps.c:41
static void set_body_state_from_quat(void)
#define FIR_FILTER_SIZE
void ahrs_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
#define AHRS_CORRECT_FREQUENCY
void ahrs_propagate(void)
Propagation.
#define AHRS_RUNNING
Definition: ahrs.h:36
struct Int32Quat ltp_to_imu_quat
float mag_zeta
filter damping for correcting the gyro bias from magnetometer.
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
euler angles