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_float_cmpl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-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 #include "subsystems/ahrs.h"
34 #include "subsystems/imu.h"
36 #include "math/pprz_algebra_int.h"
38 #include "generated/airframe.h"
39 #if USE_GPS
40 #include "subsystems/gps.h"
41 #endif
42 
43 //#include "../../test/pprz_algebra_print.h"
44 
45 #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
46 #error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
47 #endif
48 #if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
49 #error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
50 #endif
51 
52 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
53 #warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
54 #endif
55 
56 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
57 #warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
58 #endif
59 
60 #ifndef AHRS_PROPAGATE_FREQUENCY
61 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
62 #endif
64 
65 #ifndef AHRS_CORRECT_FREQUENCY
66 #define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY
67 #endif
69 
70 #ifndef AHRS_MAG_CORRECT_FREQUENCY
71 #define AHRS_MAG_CORRECT_FREQUENCY 50
72 #endif
74 
75 /*
76  * default gains for correcting attitude and bias from accel/mag
77  */
78 #ifndef AHRS_ACCEL_OMEGA
79 #define AHRS_ACCEL_OMEGA 0.063
80 #endif
81 #ifndef AHRS_ACCEL_ZETA
82 #define AHRS_ACCEL_ZETA 0.9
83 #endif
84 
85 #ifndef AHRS_MAG_OMEGA
86 #define AHRS_MAG_OMEGA 0.04
87 #endif
88 #ifndef AHRS_MAG_ZETA
89 #define AHRS_MAG_ZETA 0.9
90 #endif
91 
93 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
94 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
95 #endif
96 
97 #ifdef AHRS_UPDATE_FW_ESTIMATOR
98 // remotely settable
99 #ifndef INS_ROLL_NEUTRAL_DEFAULT
100 #define INS_ROLL_NEUTRAL_DEFAULT 0
101 #endif
102 #ifndef INS_PITCH_NEUTRAL_DEFAULT
103 #define INS_PITCH_NEUTRAL_DEFAULT 0
104 #endif
107 #endif //AHRS_UPDATE_FW_ESTIMATOR
108 
109 
110 void ahrs_update_mag_full(void);
111 void ahrs_update_mag_2d(void);
112 void ahrs_update_mag_2d_dumb(void);
113 
114 static inline void compute_body_orientation_and_rates(void);
115 
116 
118 
119 #if PERIODIC_TELEMETRY
121 
122 static void send_att(void) {
123  struct FloatEulers ltp_to_imu_euler;
125  struct Int32Eulers euler_i;
126  EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
127  struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
128  DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
129  &euler_i.phi,
130  &euler_i.theta,
131  &euler_i.psi,
132  &(eulers_body->phi),
133  &(eulers_body->theta),
134  &(eulers_body->psi));
135 }
136 
137 static void send_geo_mag(void) {
138  DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice,
140 }
141 
142 // TODO convert from float to int if we really need this one
143 /*
144 static void send_rmat(void) {
145  struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i();
146  DOWNLINK_SEND_AHRS_RMAT(DefaultChannel, DefaultDevice,
147  &ahrs_impl.ltp_to_imu_rmat.m[0],
148  &ahrs_impl.ltp_to_imu_rmat.m[1],
149  &ahrs_impl.ltp_to_imu_rmat.m[2],
150  &ahrs_impl.ltp_to_imu_rmat.m[3],
151  &ahrs_impl.ltp_to_imu_rmat.m[4],
152  &ahrs_impl.ltp_to_imu_rmat.m[5],
153  &ahrs_impl.ltp_to_imu_rmat.m[6],
154  &ahrs_impl.ltp_to_imu_rmat.m[7],
155  &ahrs_impl.ltp_to_imu_rmat.m[8],
156  &(att_rmat->m[0]),
157  &(att_rmat->m[1]),
158  &(att_rmat->m[2]),
159  &(att_rmat->m[3]),
160  &(att_rmat->m[4]),
161  &(att_rmat->m[5]),
162  &(att_rmat->m[6]),
163  &(att_rmat->m[7]),
164  &(att_rmat->m[8]));
165 }
166 */
167 #endif
168 
169 void ahrs_init(void) {
173 
174  /* Set ltp_to_imu so that body is zero */
176  sizeof(struct FloatQuat));
178  sizeof(struct FloatRMat));
179 
181 
182  /* set default filter cut-off frequency and damping */
187 
188 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
190 #else
192 #endif
193 
195 
196  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
197 
198 #if PERIODIC_TELEMETRY
199  register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
200  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
201 #endif
202 }
203 
204 void ahrs_align(void) {
205 
206 #if USE_MAGNETOMETER
207  /* Compute an initial orientation from accel and mag directly as quaternion */
210 #else
211  /* Compute an initial orientation from accel and just set heading to zero */
214 #endif
215 
216  /* Convert initial orientation from quat to rotation matrix representations. */
218 
219  /* Compute initial body orientation */
221 
222  /* used averaged gyro as initial value for bias */
223  struct Int32Rates bias0;
227 }
228 
229 
230 void ahrs_propagate(void) {
231 
232  /* converts gyro to floating point */
233  struct FloatRates gyro_float;
234  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
235  /* unbias measurement */
236  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
237 
238 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
239  const float alpha = 0.1;
240  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
241 #else
242  RATES_COPY(ahrs_impl.imu_rate,gyro_float);
243 #endif
244 
245  /* add correction */
246  struct FloatRates omega;
247  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
248  /* and zeros it */
250 
251  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
252 #if AHRS_PROPAGATE_RMAT
256 #endif
257 #if AHRS_PROPAGATE_QUAT
261 #endif
263 
264 }
265 
266 void ahrs_update_accel(void) {
267 
268  /* last column of roation matrix = ltp z-axis in imu-frame */
269  struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
272 
273  struct FloatVect3 imu_accel_float;
274  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
275 
276  struct FloatVect3 residual;
277 
278  struct FloatVect3 pseudo_gravity_measurement;
279 
281  /*
282  * centrifugal acceleration in body frame
283  * a_c_body = omega x (omega x r)
284  * (omega x r) = tangential velocity in body frame
285  * a_c_body = omega x vel_tangential_body
286  * assumption: tangential velocity only along body x-axis
287  */
288  const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
289  struct FloatVect3 acc_c_body;
290  VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
291 
292  /* convert centrifugal acceleration from body to imu frame */
293  struct FloatVect3 acc_c_imu;
294  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
295  FLOAT_RMAT_VECT3_MUL(acc_c_imu, *body_to_imu_rmat, acc_c_body);
296 
297  /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
298  VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
299 
300  } else {
301  VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
302  }
303 
304  FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
305 
306  /* FIR filtered pseudo_gravity_measurement */
307  #define FIR_FILTER_SIZE 8
308  static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
309  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
310  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
311  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
312 
314  /* heuristic on acceleration (gravity estimate) norm */
315  /* Factor how strongly to change the weight.
316  * e.g. for gravity_heuristic_factor 30:
317  * <0.66G = 0, 1G = 1.0, >1.33G = 0
318  */
319 
320  const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
321  ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
322  Bound(ahrs_impl.weight, 0.15, 1.0);
323  }
324  else {
325  ahrs_impl.weight = 1.0;
326  }
327 
328  /* Complementary filter proportional gain.
329  * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
330  */
331  const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
333  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
334 
335  /* Complementary filter integral gain
336  * Correct the gyro bias.
337  * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
338  */
339  const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
341  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
342 
343  /* FIXME: saturate bias */
344 
345 }
346 
347 
348 void ahrs_update_mag(void) {
349 #if AHRS_MAG_UPDATE_ALL_AXES
351 #else
353 #endif
354 }
355 
357 
358  struct FloatVect3 expected_imu;
360 
361  struct FloatVect3 measured_imu;
362  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
363 
364  struct FloatVect3 residual_imu;
365  FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
366  // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
367  // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
368  // DISPLAY_FLOAT_VECT3("# residual", residual);
369 
370  /* Complementary filter proportional gain.
371  * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
372  */
373 
374  const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
376  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
377 
378  /* Complementary filter integral gain
379  * Correct the gyro bias.
380  * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
381  */
382  const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
384  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
385 
386 }
387 
388 void ahrs_update_mag_2d(void) {
389 
390  struct FloatVect2 expected_ltp;
391  VECT2_COPY(expected_ltp, ahrs_impl.mag_h);
392  // normalize expected ltp in 2D (x,y)
393  FLOAT_VECT2_NORMALIZE(expected_ltp);
394 
395  struct FloatVect3 measured_imu;
396  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
397  struct FloatVect3 measured_ltp;
398  FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);
399 
400  struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y};
401 
402  // normalize measured ltp in 2D (x,y)
403  FLOAT_VECT2_NORMALIZE(measured_ltp_2d);
404 
405  const struct FloatVect3 residual_ltp =
406  { 0,
407  0,
408  measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x };
409 
410  // printf("res : %f\n", residual_ltp.z);
411 
412  struct FloatVect3 residual_imu;
413  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
414 
415 
416  /* Complementary filter proportional gain.
417  * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
418  */
419  const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
421  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
422 
423  /* Complementary filter integral gain
424  * Correct the gyro bias.
425  * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
426  */
427  const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
429  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
430 }
431 
432 
434 
435  /* project mag on local tangeant plane */
436  struct FloatEulers ltp_to_imu_euler;
438  struct FloatVect3 magf;
439  MAGS_FLOAT_OF_BFP(magf, imu.mag);
440  const float cphi = cosf(ltp_to_imu_euler.phi);
441  const float sphi = sinf(ltp_to_imu_euler.phi);
442  const float ctheta = cosf(ltp_to_imu_euler.theta);
443  const float stheta = sinf(ltp_to_imu_euler.theta);
444  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
445  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
446 
447  const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0) * me +
449  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0),
452  const float mag_rate_update_gain = 2.5;
453  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
454  const float mag_bias_update_gain = -2.5e-4;
455  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
456 
457 }
458 
459 void ahrs_update_gps(void) {
460 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
461  if (gps.fix == GPS_FIX_3D) {
464  } else {
466  }
467 #endif
468 
469 #if AHRS_USE_GPS_HEADING && USE_GPS
470  //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
471  if(gps.fix == GPS_FIX_3D &&
472  gps.gspeed >= 500 &&
473  gps.cacc <= RadOfDeg(10*1e7)) {
474 
475  // gps.course is in rad * 1e7, we need it in rad
476  float course = gps.course / 1e7;
477 
479  /* the assumption here is that there is no side-slip, so heading=course */
480  ahrs_update_heading(course);
481  }
482  else {
483  /* hard reset the heading if this is the first measurement */
484  ahrs_realign_heading(course);
485  }
486  }
487 #endif
488 }
489 
490 
492 
493  FLOAT_ANGLE_NORMALIZE(heading);
494 
495  struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
496  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
497  // we only consider x and y
498  struct FloatVect2 expected_ltp =
499  { RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
500  RMAT_ELMT(*ltp_to_body_rmat, 0, 1) };
501 
502  // expected_heading cross measured_heading
503  struct FloatVect3 residual_ltp =
504  { 0,
505  0,
506  expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)};
507 
508  struct FloatVect3 residual_imu;
509  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
510 
511  const float heading_rate_update_gain = 2.5;
512  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
513 
514  float heading_bias_update_gain;
515  /* crude attempt to only update bias if deviation is small
516  * e.g. needed when you only have gps providing heading
517  * and the inital heading is totally different from
518  * the gps course information you get once you have a gps fix.
519  * Otherwise the bias will be falsely "corrected".
520  */
521  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
522  heading_bias_update_gain = -2.5e-4;
523  else
524  heading_bias_update_gain = 0.0;
525  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
526 }
527 
528 
530  FLOAT_ANGLE_NORMALIZE(heading);
531 
532  /* quaternion representing only the heading rotation from ltp to body */
533  struct FloatQuat q_h_new;
534  q_h_new.qx = 0.0;
535  q_h_new.qy = 0.0;
536  q_h_new.qz = sinf(heading/2.0);
537  q_h_new.qi = cosf(heading/2.0);
538 
539  struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f();
540  /* quaternion representing current heading only */
541  struct FloatQuat q_h;
542  QUAT_COPY(q_h, *ltp_to_body_quat);
543  q_h.qx = 0.;
544  q_h.qy = 0.;
546 
547  /* quaternion representing rotation from current to new heading */
548  struct FloatQuat q_c;
549  FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
550 
551  /* correct current heading in body frame */
552  struct FloatQuat q;
553  FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat);
554 
555  /* compute ltp to imu rotation quaternion and matrix */
556  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
557  FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, *body_to_imu_quat);
559 
560  /* set state */
562 
564 }
565 
566 
570 static inline void compute_body_orientation_and_rates(void) {
571  /* Compute LTP to BODY quaternion */
572  struct FloatQuat ltp_to_body_quat;
573  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
574  FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
575  /* Set state */
576 #ifdef AHRS_UPDATE_FW_ESTIMATOR
577  struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. };
578  struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat;
579  FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
580  FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat);
581  FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
582  stateSetNedToBodyQuat_f(&ltp_to_neutrals_quat);
583 #else
584  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
585 #endif
586 
587  /* compute body rates */
588  struct FloatRates body_rate;
589  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
590  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate);
591  stateSetBodyRates_f(&body_rate);
592 }
#define FLOAT_RATES_ZERO(_r)
Interface to align the AHRS via low-passed measurements at startup.
int32_t phi
in rad with INT32_ANGLE_FRAC
rotation matrix
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
struct FloatRates rate_correction
Attitude and Heading Reference System interface.
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
void ahrs_realign_heading(float heading)
Hard reset yaw to a heading.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
void ahrs_update_mag_full(void)
#define AHRS_MAG_OMEGA
#define AHRS_MAG_ZETA
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Periodic telemetry system header (includes downlink utility and generated code).
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s)
angular rates
static const float dt
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
void ahrs_update_mag_2d(void)
bool_t heading_aligned
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define AHRS_CORRECT_FREQUENCY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:75
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
Utility functions for floating point AHRS implementations.
struct FloatRates gyro_bias
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)
#define INS_ROLL_NEUTRAL_DEFAULT
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:985
float ltp_vel_norm
velocity norm for gravity correction during coordinated turns
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
#define AHRS_PROPAGATE_FREQUENCY
int32_t theta
in rad with INT32_ANGLE_FRAC
struct FloatRMat ltp_to_imu_rmat
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:44
float theta
in radians
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:340
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
#define FLOAT_QUAT_OF_EULERS(_q, _e)
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
struct FloatRates imu_rate
float accel_omega
filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) ...
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1017
#define FALSE
Definition: imu_chimu.h:141
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:720
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1022
struct AhrsFloatCmpl ahrs_impl
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Roation quaternion.
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
bool_t correct_gravity
enable gravity correction during coordinated turns
#define FLOAT_VECT3_NORM(_v)
float heading
Definition: ahrs_infrared.c:40
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
Paparazzi floating point algebra.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float phi
in radians
void ahrs_update_heading(float heading)
Update yaw based on a heading measurement.
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
struct FloatQuat ltp_to_imu_quat
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
Device independent GPS code (interface)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
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
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#define FLOAT_ANGLE_NORMALIZE(_a)
void ahrs_align(void)
Aligns the AHRS.
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573
Inertial Measurement Unit interface.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:672
#define AHRS_ACCEL_OMEGA
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define AHRS_MAG_CORRECT_FREQUENCY
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
void ahrs_init(void)
AHRS initialization.
#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 FLOAT_QUAT_NORMALIZE(_q)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
float mag_zeta
filter damping for correcting the gyro bias from magnetometer
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:136
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct Int32Vect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
bool_t ltp_vel_norm_valid
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
float accel_zeta
filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) ...
#define FIR_FILTER_SIZE
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:171
int32_t psi
in rad with INT32_ANGLE_FRAC
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
Complementary filter in float to estimate the attitude, heading and gyro bias.
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
static float float_rmat_reorthogonalize(struct FloatRMat *rm)
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
float ins_roll_neutral
Definition: ins_arduimu.c:14
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:628
void ahrs_update_mag_2d_dumb(void)
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
#define FLOAT_VECT2_NORMALIZE(_v)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:50
#define AHRS_ACCEL_ZETA
struct GpsState gps
global GPS state
Definition: gps.c:41
void ahrs_propagate(void)
Propagation.
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
#define AHRS_RUNNING
Definition: ahrs.h:36
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
struct FloatVect3 mag_h
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:347
euler angles