Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
30
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"
41
42#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
43PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
44#endif
45
46#ifdef AHRS_FLOATING_HEADING
47PRINT_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
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
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
116static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt);
117static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt);
118
120{
121
122 ahrs_icq.status = AHRS_ICQ_UNINIT;
123 ahrs_icq.is_aligned = false;
124
125 ahrs_icq.ltp_vel_norm_valid = false;
126 ahrs_icq.heading_aligned = false;
127
128 /* init ltp_to_body quaternion as zero/identity rotation */
129 int32_quat_identity(&ahrs_icq.ltp_to_body_quat);
130 INT_RATES_ZERO(ahrs_icq.body_rate);
131
132 INT_RATES_ZERO(ahrs_icq.gyro_bias);
133 INT_RATES_ZERO(ahrs_icq.rate_correction);
134 INT_RATES_ZERO(ahrs_icq.high_rez_bias);
135
136 /* set default filter cut-off frequency and damping */
137 ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA;
138 ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA;
140 ahrs_icq.mag_omega = AHRS_MAG_OMEGA;
141 ahrs_icq.mag_zeta = AHRS_MAG_ZETA;
143
144 /* set default gravity heuristic */
145 ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
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
153 VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
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
161bool 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 */
172 ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_body_quat, lp_accel);
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);
181 INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28);
182
183 ahrs_icq.status = AHRS_ICQ_RUNNING;
184 ahrs_icq.is_aligned = true;
185
186 return true;
187}
188
189
190void 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 */
208 RATES_ADD(omega, ahrs_icq.rate_correction);
209 /* and zeros it */
210 INT_RATES_ZERO(ahrs_icq.rate_correction);
211
212 /* integrate quaternion */
213 int32_quat_integrate_fi(&ahrs_icq.ltp_to_body_quat, &ahrs_icq.high_rez_quat,
214 &omega, freq);
215 int32_quat_normalize(&ahrs_icq.ltp_to_body_quat);
216
217 // increase accel and mag propagation counters
218 ahrs_icq.accel_cnt++;
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 /
232 (2 * ahrs_icq.accel_omega * ahrs_icq.accel_zeta);
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 */
241 ahrs_icq.accel_inv_ki = 9.81 / 2048 / (ahrs_icq.accel_omega * ahrs_icq.accel_omega);
242}
243
244void 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
262 if (ahrs_icq.correct_gravity && ahrs_icq.ltp_vel_norm_valid) {
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 */
283 {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
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
308 if (ahrs_icq.gravity_heuristic_factor) {
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 */
341 int32_t inv_rate_scale = (int32_t)(ahrs_icq.accel_inv_kp / ahrs_icq.accel_cnt
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
375 INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
376
377}
378
379
380void 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
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 */
400 ahrs_icq.mag_kp = 2 * ahrs_icq.mag_zeta * ahrs_icq.mag_omega;
401 /* Complementary filter integral gain = omega^2 */
402 ahrs_icq.mag_ki = ahrs_icq.mag_omega * ahrs_icq.mag_omega;
403}
404
405
406static 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
454 INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
455
456}
457
458
459static 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) */
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
521 INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
522
523}
524
525void 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.);
530 ahrs_icq.ltp_vel_norm_valid = true;
531 } else {
532 ahrs_icq.ltp_vel_norm_valid = false;
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
550 if (ahrs_icq.heading_aligned) {
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
614 INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
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;
629 QUAT_COPY(q_h, ahrs_icq.ltp_to_body_quat);
630 q_h.qx = 0;
631 q_h.qy = 0;
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;
640 int32_quat_comp_norm_shortest(&q, &q_c, &ahrs_icq.ltp_to_body_quat);
641 QUAT_COPY(ahrs_icq.ltp_to_body_quat, q);
642
643 ahrs_icq.heading_aligned = true;
644}
#define AHRS_MAG_OMEGA
#define AHRS_MAG_ZETA
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
#define FIR_FILTER_SIZE
#define AHRS_ACCEL_OMEGA
#define AHRS_ACCEL_ZETA
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
#define ACC_FROM_CROSS_FRAC
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)
void ahrs_icq_realign_heading(int32_t heading)
Hard reset yaw to a heading.
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
void ahrs_icq_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
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)
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).
@ AHRS_ICQ_UNINIT
@ AHRS_ICQ_RUNNING
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]
static uint16_t c2
#define UNUSED(x)
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:90
#define FLOAT_VECT3_NORM(_v)
#define VECT3_SDIV(_vo, _vi, _s)
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
#define RATES_COPY(_a, _b)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define RATES_SMUL(_ro, _ri, _s)
#define VECT3_SMUL(_vo, _vi, _s)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define QUAT_COPY(_qo, _qi)
#define RMAT_ELMT(_rm, _row, _col)
#define RATES_ADD(_a, _b)
#define VECT3_COPY(_a, _b)
#define RATES_DIFF(_c, _a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
#define RATES_SDIV(_ro, _ri, _s)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
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")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
float heading
Definition wedgebug.c:258