Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
22 #include "subsystems/ahrs.h"
26 #include "subsystems/imu.h"
28 #include "math/pprz_algebra_int.h"
30 #include "generated/airframe.h"
31 #if USE_GPS
32 #include "subsystems/gps.h"
33 #endif
34 
35 
36 //#include "../../test/pprz_algebra_print.h"
37 
38 #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
39 #error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
40 #endif
41 #if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
42 #error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
43 #endif
44 
45 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
46 #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."
47 #endif
48 
49 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
50 #warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
51 #endif
52 
53 #ifndef AHRS_PROPAGATE_FREQUENCY
54 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
55 #endif
56 
57 void ahrs_update_mag_full(void);
58 void ahrs_update_mag_2d(void);
59 void ahrs_update_mag_2d_dumb(void);
60 
61 static inline void compute_imu_quat_and_euler_from_rmat(void);
62 static inline void compute_imu_rmat_and_euler_from_quat(void);
63 static inline void compute_body_orientation_and_rates(void);
64 
65 
67 
68 void ahrs_init(void) {
72 
73  /* Initialises IMU alignement */
74  struct FloatEulers body_to_imu_euler =
78 
79  /* Set ltp_to_body to zero */
84 
85  /* Set ltp_to_imu so that body is zero */
88  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
89 
91 
92 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
94 #else
96 #endif
97 
98 }
99 
100 void ahrs_align(void) {
101 
102 #if USE_MAGNETOMETER
103  /* Compute an initial orientation from accel and mag directly as quaternion */
106 #else
107  /* Compute an initial orientation from accel and just set heading to zero */
110 #endif
111 
112  /* Convert initial orientation from quat to euler and rotation matrix representations. */
114 
115  /* Compute initial body orientation */
117 
118  /* compute fixed point representations */
121 
122  /* used averaged gyro as initial value for bias */
123  struct Int32Rates bias0;
126 
128 }
129 
130 
131 void ahrs_propagate(void) {
132 
133  /* converts gyro to floating point */
134  struct FloatRates gyro_float;
135  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
136  /* unbias measurement */
137  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
138 
139 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
140  const float alpha = 0.1;
141  FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha);
142 #else
143  RATES_COPY(ahrs_float.imu_rate,gyro_float);
144 #endif
145 
146  /* add correction */
147  struct FloatRates omega;
148  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
149  /* and zeros it */
151 
152  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
153 #if AHRS_PROPAGATE_RMAT
157 #endif
158 #if AHRS_PROPAGATE_QUAT
162 #endif
164 
165  /* compute fixed point representations */
168 }
169 
170 void ahrs_update_accel(void) {
171 
172  /* last column of roation matrix = ltp z-axis in imu-frame */
173  struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
176 
177  struct FloatVect3 imu_accel_float;
178  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
179 
180  struct FloatVect3 residual;
181 
183  /*
184  * centrifugal acceleration in body frame
185  * a_c_body = omega x (omega x r)
186  * (omega x r) = tangential velocity in body frame
187  * a_c_body = omega x vel_tangential_body
188  * assumption: tangential velocity only along body x-axis
189  */
190  const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
191  struct FloatVect3 acc_c_body;
192  VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_float.body_rate, vel_tangential_body);
193 
194  /* convert centrifucal acceleration from body to imu frame */
195  struct FloatVect3 acc_c_imu;
196  FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
197 
198  /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
199  struct FloatVect3 corrected_gravity;
200  VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
201 
202  /* compute the residual of gravity vector in imu frame */
203  FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
204  } else {
205  FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
206  }
207 
208 #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
209  /* heuristic on acceleration norm */
210  const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
211  const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
212 #else
213  const float weight = 1.;
214 #endif
215 
216  /* compute correction */
217  const float gravity_rate_update_gain = -5e-2; // -5e-2
218  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
219 
220  const float gravity_bias_update_gain = 1e-5; // -5e-6
221  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
222 
223  /* FIXME: saturate bias */
224 
225 }
226 
227 
228 void ahrs_update_mag(void) {
229 #if AHRS_MAG_UPDATE_ALL_AXES
231 #else
233 #endif
234 }
235 
237 
238  const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
239  struct FloatVect3 expected_imu;
240  FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
241 
242  struct FloatVect3 measured_imu;
243  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
244 
245  struct FloatVect3 residual_imu;
246  FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
247  // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
248  // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
249  // DISPLAY_FLOAT_VECT3("# residual", residual);
250 
251  const float mag_rate_update_gain = 2.5;
252  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
253 
254  const float mag_bias_update_gain = -2.5e-3;
255  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
256 
257 }
258 
259 void ahrs_update_mag_2d(void) {
260 
261  const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
262 
263  struct FloatVect3 measured_imu;
264  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
265  struct FloatVect3 measured_ltp;
266  FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu);
267 
268  const struct FloatVect3 residual_ltp =
269  { 0,
270  0,
271  measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
272 
273  // printf("res : %f\n", residual_ltp.z);
274 
275  struct FloatVect3 residual_imu;
276  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);
277 
278  const float mag_rate_update_gain = 2.5;
279  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
280 
281  const float mag_bias_update_gain = -2.5e-3;
282  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
283 
284 }
285 
286 
288 
289  /* project mag on local tangeant plane */
290  struct FloatVect3 magf;
291  MAGS_FLOAT_OF_BFP(magf, imu.mag);
292  const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi);
293  const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi);
294  const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta);
295  const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta);
296  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
297  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
298 
299  const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
300  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0),
303  const float mag_rate_update_gain = 2.5;
304  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
305  const float mag_bias_update_gain = -2.5e-4;
306  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
307 
308 }
309 
310 void ahrs_update_gps(void) {
311 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
312  if (gps.fix == GPS_FIX_3D) {
315  } else {
317  }
318 #endif
319 
320 #if AHRS_USE_GPS_HEADING && USE_GPS
321  //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
322  if(gps.fix == GPS_FIX_3D &&
323  gps.gspeed >= 500 &&
324  gps.cacc <= RadOfDeg(10*1e7)) {
325 
326  // gps.course is in rad * 1e7, we need it in rad
327  float course = gps.course / 1e7;
328 
330  /* the assumption here is that there is no side-slip, so heading=course */
331  ahrs_update_heading(course);
332  }
333  else {
334  /* hard reset the heading if this is the first measurement */
335  ahrs_realign_heading(course);
336  }
337  }
338 #endif
339 }
340 
341 
342 void ahrs_update_heading(float heading) {
343 
344  FLOAT_ANGLE_NORMALIZE(heading);
345 
346  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
347  // we only consider x and y
348  struct FloatVect2 expected_ltp =
351 
352  // expected_heading cross measured_heading
353  struct FloatVect3 residual_ltp =
354  { 0,
355  0,
356  expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)};
357 
358  struct FloatVect3 residual_imu;
359  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);
360 
361  const float heading_rate_update_gain = 2.5;
362  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
363 
364  float heading_bias_update_gain;
365  /* crude attempt to only update bias if deviation is small
366  * e.g. needed when you only have gps providing heading
367  * and the inital heading is totally different from
368  * the gps course information you get once you have a gps fix.
369  * Otherwise the bias will be falsely "corrected".
370  */
371  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
372  heading_bias_update_gain = -2.5e-4;
373  else
374  heading_bias_update_gain = 0.0;
375  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
376 }
377 
378 
379 void ahrs_realign_heading(float heading) {
380  FLOAT_ANGLE_NORMALIZE(heading);
381 
382  /* quaternion representing only the heading rotation from ltp to body */
383  struct FloatQuat q_h_new;
384  q_h_new.qx = 0.0;
385  q_h_new.qy = 0.0;
386  q_h_new.qz = sinf(heading/2.0);
387  q_h_new.qi = cosf(heading/2.0);
388 
389  /* quaternion representing current heading only */
390  struct FloatQuat q_h;
392  q_h.qx = 0.;
393  q_h.qy = 0.;
395 
396  /* quaternion representing rotation from current to new heading */
397  struct FloatQuat q_c;
398  FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
399 
400  /* correct current heading in body frame */
401  struct FloatQuat q;
404 
405  /* compute ltp to imu rotation quaternion */
408 
409  /* compute other body orientation representations */
412 
413  /* compute other ltp to imu rotation representations */
415 
416  /* compute fixed point representations */
419 
421 }
422 
423 
428 static inline void compute_imu_quat_and_euler_from_rmat(void) {
431 }
432 
433 
434 static inline void compute_imu_rmat_and_euler_from_quat(void) {
437 }
438 
439 
440 
444 static inline void compute_body_orientation_and_rates(void) {
451 }
452 
453 
454 #ifdef AHRS_UPDATE_FW_ESTIMATOR
455 // TODO use ahrs result directly
456 #include "estimator.h"
457 // remotely settable
458 #ifndef INS_ROLL_NEUTRAL_DEFAULT
459 #define INS_ROLL_NEUTRAL_DEFAULT 0
460 #endif
461 #ifndef INS_PITCH_NEUTRAL_DEFAULT
462 #define INS_PITCH_NEUTRAL_DEFAULT 0
463 #endif
464 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
465 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
466 void ahrs_update_fw_estimator(void)
467 {
468  // export results to estimator
472 
476 }
477 #endif //AHRS_UPDATE_FW_ESTIMATOR
#define FLOAT_RATES_ZERO(_r)
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:82
struct FloatRMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:68
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:59
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Attitude and Heading Reference System interface.
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
void ahrs_realign_heading(float heading)
Hard reset yaw to a heading.
float estimator_r
Definition: estimator.c:56
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
#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.
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:61
struct FloatRates gyro_bias
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s)
angular rates
#define FLOAT_RMAT_ZERO(_rm)
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float psi
in radians
void ahrs_update_mag_2d(void)
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:36
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:303
#define AHRS_INT_OF_FLOAT()
Definition: ahrs.h:89
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:74
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define AHRS_PROPAGATE_FREQUENCY
struct FloatQuat body_to_imu_quat
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:43
float theta
in radians
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:317
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
euler angles
#define FLOAT_QUAT_OF_EULERS(_q, _e)
float estimator_p
Definition: estimator.c:54
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:37
#define FALSE
Definition: imu_chimu.h:141
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:643
float p
in rad/s^2
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Roation quaternion.
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
#define FLOAT_VECT3_NORM(_v)
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:30
Paparazzi floating point algebra.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float phi
in radians
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:564
void ahrs_update_heading(float heading)
Update yaw based on a heading measurement.
void ahrs_update_gps(void)
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
Definition: pprz_algebra.h:223
struct AhrsFloatCmplRmat ahrs_impl
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:520
Inertial Measurement Unit interface.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:619
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:62
float ins_pitch_neutral
Definition: ins_arduimu.c:15
float r
in rad/s^2
#define AHRS_H_Y
static void compute_imu_rmat_and_euler_from_quat(void)
void ahrs_init(void)
AHRS initialization.
#define AHRS_UNINIT
Definition: ahrs.h:33
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define FLOAT_QUAT_NORMALIZE(_q)
void ahrs_update_fw_estimator(void)
#define FLOAT_EULERS_ZERO(_e)
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
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)
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:478
#define FLOAT_QUAT_OF_RMAT(_q, _r)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:155
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:631
struct FloatRates rate_correction
#define AHRS_IMU_INT_OF_FLOAT()
Definition: ahrs.h:96
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
State estimation, fusioning sensors.
static float float_rmat_reorthogonalize(struct FloatRMat *rm)
static void compute_imu_quat_and_euler_from_rmat(void)
Compute ltp to imu rotation in euler angles and quaternion representations from the rotation matrix r...
float q
in rad/s^2
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:83
void ahrs_update_mag_2d_dumb(void)
#define FLOAT_QUAT_ZERO(_q)
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:81
struct GpsState gps
global GPS state
Definition: gps.c:31
void ahrs_propagate(void)
Propagation.
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:236
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
#define AHRS_RUNNING
Definition: ahrs.h:34
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:35
Paparazzi fixed point algebra.
struct FloatRMat body_to_imu_rmat
struct FloatEulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:60
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:324