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_int_cmpl_quat.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 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 
26 
27 #include "subsystems/imu.h"
28 #if USE_GPS
29 #include "subsystems/gps.h"
30 #endif
31 #include "math/pprz_trig_int.h"
32 #include "math/pprz_algebra_int.h"
33 
34 #include "generated/airframe.h"
35 
36 //#include "../../test/pprz_algebra_print.h"
37 
38 static inline void ahrs_update_mag_full(void);
39 static inline void ahrs_update_mag_2d(void);
40 
41 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
42 #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."
43 #endif
44 
45 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
46 #warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
47 #endif
48 
49 #ifndef AHRS_PROPAGATE_FREQUENCY
50 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
51 #endif
52 
54 
55 static inline void compute_imu_euler_and_rmat_from_quat(void);
56 static inline void compute_body_euler_and_rmat_from_quat(void);
57 static inline void compute_imu_orientation(void);
58 static inline void compute_body_orientation(void);
59 
60 void ahrs_init(void) {
61 
65 
66  /* set ltp_to_body to zero */
71 
72  /* set ltp_to_imu so that body is zero */
77 
81 
82 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
84 #else
86 #endif
87 
88 #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
90 #else
92 #endif
93 
94 }
95 
96 void ahrs_align(void) {
97 
98 #if USE_MAGNETOMETER
99  /* Compute an initial orientation from accel and mag directly as quaternion */
102 #else
103  /* Compute an initial orientation from accel and just set heading to zero */
106 #endif
107 
108  /* Convert initial orientation from quat to euler and rotation matrix representations. */
110 
112 
113  /* Use low passed gyro value as initial bias */
117 
119 }
120 
121 
122 
123 /*
124  *
125  *
126  *
127  */
128 void ahrs_propagate(void) {
129 
130  /* unbias gyro */
131  struct Int32Rates omega;
133 
134  /* low pass rate */
135 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
137  RATES_ADD(ahrs.imu_rate, omega);
139 #else
140  RATES_COPY(ahrs.imu_rate, omega);
141 #endif
142 
143  /* add correction */
145  /* and zeros it */
147 
148  /* integrate quaternion */
151 
153 
155 
156 }
157 
158 
159 
160 
161 void ahrs_update_accel(void) {
162 
163  // c2 = ltp z-axis in imu-frame
164  struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
167  struct Int32Vect3 residual;
168 
170  /*
171  * centrifugal acceleration in body frame
172  * a_c_body = omega x (omega x r)
173  * (omega x r) = tangential velocity in body frame
174  * a_c_body = omega x vel_tangential_body
175  * assumption: tangential velocity only along body x-axis
176  */
177 
178  // FIXME: check overflows !
179  const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0};
180  struct Int32Vect3 acc_c_body;
181  VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
183 
184  /* convert centrifucal acceleration from body to imu frame */
185  struct Int32Vect3 acc_c_imu;
186  INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
187 
188  /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
189  struct Int32Vect3 corrected_gravity;
190  INT32_VECT3_DIFF(corrected_gravity, imu.accel, acc_c_imu);
191 
192  /* compute the residual of gravity vector in imu frame */
193  INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
194  } else {
195  INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
196  }
197 
198 
199  int32_t inv_weight;
201  /* heuristic on acceleration norm */
202  int32_t acc_norm;
203  INT32_VECT3_NORM(acc_norm, imu.accel);
204  const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm);
205  inv_weight = Chop(6*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 6);
206  }
207  else {
208  inv_weight = 1;
209  }
210 
211  // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
212  // rate_correction FRAC = RATE_FRAC = 12
213  // 2^12 / 2^24 * 5e-2 = 1/81920
214  ahrs_impl.rate_correction.p += -residual.x/82000/inv_weight;
215  ahrs_impl.rate_correction.q += -residual.y/82000/inv_weight;
216  ahrs_impl.rate_correction.r += -residual.z/82000/inv_weight;
217 
218  // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
219  // high_rez_bias = RATE_FRAC+28 = 40
220  // 2^40 / 2^24 * 5e-6 = 1/3.05
221 
222  // ahrs_impl.high_rez_bias.p += residual.x*3;
223  // ahrs_impl.high_rez_bias.q += residual.y*3;
224  // ahrs_impl.high_rez_bias.r += residual.z*3;
225 
226  ahrs_impl.high_rez_bias.p += residual.x/inv_weight;
227  ahrs_impl.high_rez_bias.q += residual.y/inv_weight;
228  ahrs_impl.high_rez_bias.r += residual.z/inv_weight;
229 
230 
231  /* */
233 
234 }
235 
236 void ahrs_update_mag(void) {
237 #if AHRS_MAG_UPDATE_ALL_AXES
239 #else
241 #endif
242 }
243 
244 
245 static inline void ahrs_update_mag_full(void) {
246  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
248  MAG_BFP_OF_REAL(AHRS_H_Z)};
249  struct Int32Vect3 expected_imu;
250  INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
251 
252  struct Int32Vect3 residual;
253  INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
254 
255  ahrs_impl.rate_correction.p += residual.x/32/16;
256  ahrs_impl.rate_correction.q += residual.y/32/16;
257  ahrs_impl.rate_correction.r += residual.z/32/16;
258 
259 
260  ahrs_impl.high_rez_bias.p -= residual.x/32*1024;
261  ahrs_impl.high_rez_bias.q -= residual.y/32*1024;
262  ahrs_impl.high_rez_bias.r -= residual.z/32*1024;
263 
264 
266 
267 }
268 
269 
270 static inline void ahrs_update_mag_2d(void) {
271 
272  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
274 
275  struct Int32Vect3 measured_ltp;
277 
278  struct Int32Vect3 residual_ltp =
279  { 0,
280  0,
281  (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
282 
283  struct Int32Vect3 residual_imu;
284  INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
285 
286  // residual_ltp FRAC = 2 * MAG_FRAC = 22
287  // rate_correction FRAC = RATE_FRAC = 12
288  // 2^12 / 2^22 * 2.5 = 1/410
289 
290  // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
291  // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
292  // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
293 
294  ahrs_impl.rate_correction.p += residual_imu.x/16;
295  ahrs_impl.rate_correction.q += residual_imu.y/16;
296  ahrs_impl.rate_correction.r += residual_imu.z/16;
297 
298 
299  // residual_ltp FRAC = 2 * MAG_FRAC = 22
300  // high_rez_bias = RATE_FRAC+28 = 40
301  // 2^40 / 2^22 * 2.5e-3 = 655
302 
303  // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
304  // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
305  // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
306 
307  ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
308  ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
309  ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
310 
311 
313 
314 }
315 
316 void ahrs_update_gps(void) {
317 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
318  if (gps.fix == GPS_FIX_3D) {
321  } else {
323  }
324 #endif
325 
326 #if AHRS_USE_GPS_HEADING && USE_GPS
327  //got a 3d fix,ground speed > 0.5 m/s and course accuracy is better than 10deg
328  if(gps.fix == GPS_FIX_3D &&
329  gps.gspeed >= 500 &&
330  gps.cacc <= RadOfDeg(10*1e7)) {
331 
332  // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
333  int32_t course = gps.course * ((1<<INT32_ANGLE_FRAC) / 1e7);
334 
335  /* the assumption here is that there is no side-slip, so heading=course */
336 
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  INT32_ANGLE_NORMALIZE(heading);
352 
353  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
354  // we only consider x and y
355  struct Int32Vect2 expected_ltp =
358 
359  int32_t heading_x, heading_y;
360  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
361  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
362 
363  // expected_heading cross measured_heading ??
364  struct Int32Vect3 residual_ltp =
365  { 0,
366  0,
367  (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<<INT32_ANGLE_FRAC)};
368 
369  struct Int32Vect3 residual_imu;
370  INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
371 
372  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
373  // rate_correction FRAC = RATE_FRAC = 12
374  // 2^12 / 2^28 * 4.0 = 1/2^14
375  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
376  ahrs_impl.rate_correction.p += residual_imu.x/4;
377  ahrs_impl.rate_correction.q += residual_imu.y/4;
378  ahrs_impl.rate_correction.r += residual_imu.z/4;
379 
380 
381  /* crude attempt to only update bias if deviation is small
382  * e.g. needed when you only have gps providing heading
383  * and the inital heading is totally different from
384  * the gps course information you get once you have a gps fix.
385  * Otherwise the bias will be falsely "corrected".
386  */
387  int32_t sin_max_angle_deviation;
388  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(5.)));
389  if (ABS(residual_ltp.z) < sin_max_angle_deviation)
390  {
391  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
392  // high_rez_bias = RATE_FRAC+28 = 40
393  // 2^40 / 2^28 * 2.5e-4 = 1
394  ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<INT32_ANGLE_FRAC);
395  ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<INT32_ANGLE_FRAC);
396  ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<INT32_ANGLE_FRAC);
397 
399  }
400 }
401 
403 
404  /* quaternion representing only the heading rotation from ltp to body */
405  struct Int32Quat q_h_new;
406  q_h_new.qx = 0;
407  q_h_new.qy = 0;
408  PPRZ_ITRIG_SIN(q_h_new.qz, heading/2);
409  PPRZ_ITRIG_COS(q_h_new.qi, heading/2);
410 
411  /* quaternion representing current heading only */
412  struct Int32Quat q_h;
414  q_h.qx = 0;
415  q_h.qy = 0;
417 
418  /* quaternion representing rotation from current to new heading */
419  struct Int32Quat q_c;
420  INT32_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
421 
422  /* correct current heading in body frame */
423  struct Int32Quat q;
426 
427  /* compute other representations in body frame */
429 
430  /* compute ltp to imu rotations */
432 
434 }
435 
436 
437 /* Compute ltp to imu rotation in euler angles and rotation matrix representation
438  from the quaternion representation */
439 __attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
440 
441  /* Compute LTP to IMU euler */
443  /* Compute LTP to IMU rotation matrix */
445 
446 }
447 
448 /* Compute ltp to body rotation in euler angles and rotation matrix representation
449  from the quaternion representation */
450 __attribute__ ((always_inline)) static inline void compute_body_euler_and_rmat_from_quat(void) {
451 
452  /* Compute LTP to body euler */
454  /* Compute LTP to body rotation matrix */
456 
457 }
458 
459 __attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
460 
461  /* Compute LTP to BODY quaternion */
463  /* Compute LTP to BODY rotation matrix */
465  /* compute LTP to BODY eulers */
467  /* compute body rates */
469 
470 }
471 
472 __attribute__ ((always_inline)) static inline void compute_imu_orientation(void) {
473 
474  /* Compute LTP to IMU quaternion */
476  /* Compute LTP to IMU rotation matrix */
478  /* compute LTP to IMU eulers */
480  /* compute IMU rates */
482 
483 }
484 
485 
486 #ifdef AHRS_UPDATE_FW_ESTIMATOR
487 // TODO use ahrs result directly
488 #include "estimator.h"
489 // remotely settable
490 #ifndef INS_ROLL_NEUTRAL_DEFAULT
491 #define INS_ROLL_NEUTRAL_DEFAULT 0
492 #endif
493 #ifndef INS_PITCH_NEUTRAL_DEFAULT
494 #define INS_PITCH_NEUTRAL_DEFAULT 0
495 #endif
496 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
497 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
498 void ahrs_update_fw_estimator(void)
499 {
500  struct FloatEulers att;
501  // export results to estimator
503 
504  estimator_phi = att.phi - ins_roll_neutral;
505  estimator_theta = att.theta - ins_pitch_neutral;
506  estimator_psi = att.psi;
507 
508  struct FloatRates rates;
510  estimator_p = rates.p;
511  estimator_q = rates.q;
512  estimator_r = rates.r;
513 
514 }
515 #endif //AHRS_UPDATE_FW_ESTIMATOR
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:338
bool_t use_gravity_heuristic
static void ahrs_update_mag_2d(void)
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_r
Definition: estimator.c:56
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
void ahrs_realign_heading(int32_t heading)
Hard reset yaw to a heading.
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
int32_t p
in rad/s^2 with INT32_RATE_FRAC
static void compute_imu_euler_and_rmat_from_quat(void)
#define INT32_QUAT_ZERO(_q)
angular rates
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
#define AHRS_PROPAGATE_FREQUENCY
Rotation quaternion.
struct Int32Rates gyro_bias
#define ACCEL_BFP_OF_REAL(_af)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:36
void ahrs_init(void)
AHRS initialization.
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:303
#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:74
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
void ahrs_align(void)
Aligns the AHRS.
#define INT_RATES_ZERO(_e)
#define INT_EULERS_ZERO(_e)
bool_t heading_aligned
#define INT32_RATE_FRAC
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:43
int64_t r
in rad/s^2 with INT32_RATE_FRAC
__attribute__((always_inline))
uint8_t fix
status of fix
Definition: gps.h:77
struct Int32Rates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:47
#define GPS_FIX_3D
Definition: gps.h:42
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
euler angles
#define INT32_VECT3_NORM(n, v)
float estimator_p
Definition: estimator.c:54
#define INT_RATES_RSHIFT(_o, _i, _r)
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:37
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:345
#define FALSE
Definition: imu_chimu.h:141
static void compute_imu_orientation(void)
struct Int32RMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:51
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:352
struct Int32Quat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:44
struct Int32Rates rate_correction
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
static void ahrs_update_mag_full(void)
#define MAG_BFP_OF_REAL(_af)
void ahrs_update_heading(int32_t heading)
Update yaw based on a heading measurement.
bool_t ltp_vel_norm_valid
#define INT32_ANGLE_FRAC
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:30
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:564
#define TRIG_BFP_OF_REAL(_tf)
#define INT32_RMAT_ZERO(_rm)
#define INT32_VECT3_RSHIFT(_o, _i, _r)
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
struct Int32Quat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:49
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
Definition: pprz_algebra.h:223
#define INT32_EULERS_OF_RMAT(_e, _rm)
#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:310
static void compute_body_orientation(void)
struct Int64Rates high_rez_bias
bool_t correct_gravity
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:520
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
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:619
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
#define AHRS_H_Y
signed long int32_t
Definition: types.h:19
int64_t p
in rad/s^2 with INT32_RATE_FRAC
#define AHRS_UNINIT
Definition: ahrs.h:33
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
void ahrs_update_fw_estimator(void)
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
#define INT32_RMAT_OF_QUAT(_rm, _q)
#define AHRS_H_X
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct Int32Quat body_to_imu_quat
rotation from body to imu frame as a unit quaternion
Definition: imu.h:51
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:478
struct Int32Eulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:45
struct AhrsIntCmpl ahrs_impl
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
State estimation, fusioning sensors.
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:569
int32_t q
in rad/s^2 with INT32_RATE_FRAC
static void compute_body_euler_and_rmat_from_quat(void)
int64_t q
in rad/s^2 with INT32_RATE_FRAC
#define INT32_SPEED_FRAC
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float ins_roll_neutral
Definition: ins_arduimu.c:14
int32_t r
in rad/s^2 with INT32_RATE_FRAC
void ahrs_update_gps(void)
int32_t ltp_vel_norm
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)
#define INT_RATES_LSHIFT(_o, _i, _r)
#define INT32_ACCEL_FRAC
struct Int32RMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:46
#define INT32_EULERS_OF_QUAT(_e, _q)
#define SPEED_BFP_OF_REAL(_af)
struct GpsState gps
global GPS state
Definition: gps.c:31
struct Int64Quat high_rez_quat
void ahrs_propagate(void)
Propagation.
#define AHRS_RUNNING
Definition: ahrs.h:34
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:35
Paparazzi fixed point algebra.
#define INT32_RMAT_RATEMULT(_vb, _m_a2b, _va)