Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 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 
44 //#include "../../test/pprz_algebra_print.h"
45 
46 #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
47 #error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
48 #endif
49 #if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
50 #error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
51 #endif
52 
53 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
54 #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."
55 #endif
56 
57 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
58 #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."
59 #endif
60 
61 #ifndef AHRS_PROPAGATE_FREQUENCY
62 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
63 #endif
64 
65 #ifdef AHRS_UPDATE_FW_ESTIMATOR
66 // remotely settable
67 #ifndef INS_ROLL_NEUTRAL_DEFAULT
68 #define INS_ROLL_NEUTRAL_DEFAULT 0
69 #endif
70 #ifndef INS_PITCH_NEUTRAL_DEFAULT
71 #define INS_PITCH_NEUTRAL_DEFAULT 0
72 #endif
75 #endif //AHRS_UPDATE_FW_ESTIMATOR
76 
77 
78 void ahrs_update_mag_full(void);
79 void ahrs_update_mag_2d(void);
80 void ahrs_update_mag_2d_dumb(void);
81 
82 static inline void compute_body_orientation_and_rates(void);
83 
84 
86 
87 void ahrs_init(void) {
91 
92  /* Initialises IMU alignement */
93  struct FloatEulers body_to_imu_euler =
97 
98  /* Set ltp_to_imu so that body is zero */
101 
103 
104 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
106 #else
108 #endif
109 
110 }
111 
112 void ahrs_align(void) {
113 
114 #if USE_MAGNETOMETER
115  /* Compute an initial orientation from accel and mag directly as quaternion */
118 #else
119  /* Compute an initial orientation from accel and just set heading to zero */
122 #endif
123 
124  /* Convert initial orientation from quat to rotation matrix representations. */
126 
127  /* Compute initial body orientation */
129 
130  /* used averaged gyro as initial value for bias */
131  struct Int32Rates bias0;
134 
136 }
137 
138 
139 void ahrs_propagate(void) {
140 
141  /* converts gyro to floating point */
142  struct FloatRates gyro_float;
143  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
144  /* unbias measurement */
145  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
146 
147 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
148  const float alpha = 0.1;
149  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
150 #else
151  RATES_COPY(ahrs_impl.imu_rate,gyro_float);
152 #endif
153 
154  /* add correction */
155  struct FloatRates omega;
156  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
157  /* and zeros it */
159 
160  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
161 #if AHRS_PROPAGATE_RMAT
165 #endif
166 #if AHRS_PROPAGATE_QUAT
170 #endif
172 
173 }
174 
175 void ahrs_update_accel(void) {
176 
177  /* last column of roation matrix = ltp z-axis in imu-frame */
178  struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
181 
182  struct FloatVect3 imu_accel_float;
183  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
184 
185  struct FloatVect3 residual;
186 
188  /*
189  * centrifugal acceleration in body frame
190  * a_c_body = omega x (omega x r)
191  * (omega x r) = tangential velocity in body frame
192  * a_c_body = omega x vel_tangential_body
193  * assumption: tangential velocity only along body x-axis
194  */
195  const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
196  struct FloatVect3 acc_c_body;
197  VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
198 
199  /* convert centrifugal acceleration from body to imu frame */
200  struct FloatVect3 acc_c_imu;
201  FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
202 
203  /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
204  struct FloatVect3 corrected_gravity;
205  VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
206 
207  /* compute the residual of gravity vector in imu frame */
208  FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
209  } else {
210  FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
211  }
212 
213 #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
214  /* heuristic on acceleration norm */
215  const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
216  const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
217 #else
218  const float weight = 1.;
219 #endif
220 
221  /* compute correction */
222  const float gravity_rate_update_gain = -5e-2; // -5e-2
223  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
224 
225  const float gravity_bias_update_gain = 1e-5; // -5e-6
226  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
227 
228  /* FIXME: saturate bias */
229 
230 }
231 
232 
233 void ahrs_update_mag(void) {
234 #if AHRS_MAG_UPDATE_ALL_AXES
236 #else
238 #endif
239 }
240 
242 
243  const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
244  struct FloatVect3 expected_imu;
245  FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, expected_ltp);
246 
247  struct FloatVect3 measured_imu;
248  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
249 
250  struct FloatVect3 residual_imu;
251  FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
252  // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
253  // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
254  // DISPLAY_FLOAT_VECT3("# residual", residual);
255 
256  const float mag_rate_update_gain = 2.5;
257  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
258 
259  const float mag_bias_update_gain = -2.5e-3;
260  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
261 
262 }
263 
264 void ahrs_update_mag_2d(void) {
265 
266  const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
267 
268  struct FloatVect3 measured_imu;
269  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
270  struct FloatVect3 measured_ltp;
271  FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);
272 
273  const struct FloatVect3 residual_ltp =
274  { 0,
275  0,
276  measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
277 
278  // printf("res : %f\n", residual_ltp.z);
279 
280  struct FloatVect3 residual_imu;
281  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
282 
283  const float mag_rate_update_gain = 2.5;
284  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
285 
286  const float mag_bias_update_gain = -2.5e-3;
287  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
288 
289 }
290 
291 
293 
294  /* project mag on local tangeant plane */
295  struct FloatEulers ltp_to_imu_euler;
297  struct FloatVect3 magf;
298  MAGS_FLOAT_OF_BFP(magf, imu.mag);
299  const float cphi = cosf(ltp_to_imu_euler.phi);
300  const float sphi = sinf(ltp_to_imu_euler.phi);
301  const float ctheta = cosf(ltp_to_imu_euler.theta);
302  const float stheta = sinf(ltp_to_imu_euler.theta);
303  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
304  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
305 
306  const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0)*mn;
307  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0),
310  const float mag_rate_update_gain = 2.5;
311  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
312  const float mag_bias_update_gain = -2.5e-4;
313  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
314 
315 }
316 
317 void ahrs_update_gps(void) {
318 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
319  if (gps.fix == GPS_FIX_3D) {
322  } else {
324  }
325 #endif
326 
327 #if AHRS_USE_GPS_HEADING && USE_GPS
328  //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
329  if(gps.fix == GPS_FIX_3D &&
330  gps.gspeed >= 500 &&
331  gps.cacc <= RadOfDeg(10*1e7)) {
332 
333  // gps.course is in rad * 1e7, we need it in rad
334  float course = gps.course / 1e7;
335 
337  /* the assumption here is that there is no side-slip, so heading=course */
338  ahrs_update_heading(course);
339  }
340  else {
341  /* hard reset the heading if this is the first measurement */
342  ahrs_realign_heading(course);
343  }
344  }
345 #endif
346 }
347 
348 
350 
351  FLOAT_ANGLE_NORMALIZE(heading);
352 
353  struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
354  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
355  // we only consider x and y
356  struct FloatVect2 expected_ltp =
357  { RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
358  RMAT_ELMT(*ltp_to_body_rmat, 0, 1) };
359 
360  // expected_heading cross measured_heading
361  struct FloatVect3 residual_ltp =
362  { 0,
363  0,
364  expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)};
365 
366  struct FloatVect3 residual_imu;
367  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
368 
369  const float heading_rate_update_gain = 2.5;
370  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
371 
372  float heading_bias_update_gain;
373  /* crude attempt to only update bias if deviation is small
374  * e.g. needed when you only have gps providing heading
375  * and the inital heading is totally different from
376  * the gps course information you get once you have a gps fix.
377  * Otherwise the bias will be falsely "corrected".
378  */
379  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
380  heading_bias_update_gain = -2.5e-4;
381  else
382  heading_bias_update_gain = 0.0;
383  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
384 }
385 
386 
388  FLOAT_ANGLE_NORMALIZE(heading);
389 
390  /* quaternion representing only the heading rotation from ltp to body */
391  struct FloatQuat q_h_new;
392  q_h_new.qx = 0.0;
393  q_h_new.qy = 0.0;
394  q_h_new.qz = sinf(heading/2.0);
395  q_h_new.qi = cosf(heading/2.0);
396 
397  struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f();
398  /* quaternion representing current heading only */
399  struct FloatQuat q_h;
400  QUAT_COPY(q_h, *ltp_to_body_quat);
401  q_h.qx = 0.;
402  q_h.qy = 0.;
404 
405  /* quaternion representing rotation from current to new heading */
406  struct FloatQuat q_c;
407  FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
408 
409  /* correct current heading in body frame */
410  struct FloatQuat q;
411  FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat);
412 
413  /* compute ltp to imu rotation quaternion and matrix */
416 
417  /* set state */
419 
421 }
422 
423 
427 static inline void compute_body_orientation_and_rates(void) {
428  /* Compute LTP to BODY quaternion */
429  struct FloatQuat ltp_to_body_quat;
431  /* Set state */
432 #ifdef AHRS_UPDATE_FW_ESTIMATOR
433  struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. };
434  struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat;
435  FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
436  FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat);
437  FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
438  stateSetNedToBodyQuat_f(&ltp_to_neutrals_quat);
439 #else
440  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
441 #endif
442 
443  /* compute body rates */
444  struct FloatRates body_rate;
446  stateSetBodyRates_f(&body_rate);
447 }
448 
449 
#define FLOAT_RATES_ZERO(_r)
Interface to align the AHRS via low-passed measurements at startup.
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:82
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)
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s)
angular rates
void ahrs_update_mag_2d(void)
bool_t heading_aligned
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
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
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define AHRS_PROPAGATE_FREQUENCY
struct FloatRMat ltp_to_imu_rmat
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:43
float theta
in radians
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:315
uint8_t fix
status of fix
Definition: gps.h:78
struct FloatRMat body_to_imu_rmat
#define GPS_FIX_3D
Definition: gps.h:43
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
euler angles
#define FLOAT_QUAT_OF_EULERS(_q, _e)
struct FloatQuat body_to_imu_quat
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
struct FloatRates imu_rate
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:653
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
#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
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:562
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:50
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:221
#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.
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:518
Inertial Measurement Unit interface.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:617
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define AHRS_H_Y
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
Definition: imu.h:42
#define FLOAT_QUAT_NORMALIZE(_q)
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 AHRS_H_X
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
bool_t ltp_vel_norm_valid
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:476
#define FLOAT_QUAT_OF_RMAT(_q, _r)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:153
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:641
Complementary filter in float to estimate the attitude, heading and gyro bias.
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
static float float_rmat_reorthogonalize(struct FloatRMat *rm)
#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 IMU_BODY_TO_IMU_PSI
Definition: imu.h:83
void ahrs_update_mag_2d_dumb(void)
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:81
struct GpsState gps
global GPS state
Definition: gps.c:33
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
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:322