Paparazzi UAS  v4.2.2_stable-4-gcc32f65
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 
61 void ahrs_init(void) {
62 
66 
67  /* set ltp_to_body to zero */
72 
73  /* set ltp_to_imu so that body is zero */
78 
82 
83 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
85 #else
87 #endif
88 
89 #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
91 #else
93 #endif
94 
96 
97 }
98 
99 void ahrs_align(void) {
100 
101 #if USE_MAGNETOMETER
102  /* Compute an initial orientation from accel and mag directly as quaternion */
105 #else
106  /* Compute an initial orientation from accel and just set heading to zero */
109 #endif
110 
111  /* Convert initial orientation from quat to euler and rotation matrix representations. */
113 
115 
116  /* Use low passed gyro value as initial bias */
120 
122 }
123 
124 
125 
126 /*
127  *
128  *
129  *
130  */
131 void ahrs_propagate(void) {
132 
133  /* unbias gyro */
134  struct Int32Rates omega;
136 
137  /* low pass rate */
138 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
140  RATES_ADD(ahrs.imu_rate, omega);
142 #else
143  RATES_COPY(ahrs.imu_rate, omega);
144 #endif
145 
146  /* add correction */
148  /* and zeros it */
150 
151  /* integrate quaternion */
154 
156 
158 
159 }
160 
161 
162 
163 
164 void ahrs_update_accel(void) {
165 
166  // c2 = ltp z-axis in imu-frame
167  struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
170  struct Int32Vect3 residual;
171 
172  struct Int32Vect3 pseudo_gravity_measurement;
173 
175  /*
176  * centrifugal acceleration in body frame
177  * a_c_body = omega x (omega x r)
178  * (omega x r) = tangential velocity in body frame
179  * a_c_body = omega x vel_tangential_body
180  * assumption: tangential velocity only along body x-axis
181  */
182 
183  // FIXME: check overflows !
184 #define COMPUTATION_FRAC 16
185 
186  const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
187  struct Int32Vect3 acc_c_body;
188  VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
190 
191  /* convert centrifucal acceleration from body to imu frame */
192  struct Int32Vect3 acc_c_imu;
193  INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
194 
195  /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
196  INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
197  } else {
198  VECT3_COPY(pseudo_gravity_measurement, imu.accel);
199  }
200 
201  /* compute the residual of the pseudo gravity vector in imu frame */
202  INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
203 
204 
205  int32_t inv_weight;
207  /* heuristic on acceleration norm */
208 
209  /* FIR filtered pseudo_gravity_measurement */
210  static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
211  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7);
212  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
213  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8);
214 
215  int32_t acc_norm;
216  INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement);
217  const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm);
218  inv_weight = Chop(50*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 50);
219  }
220  else {
221  inv_weight = 1;
222  }
223 
224  // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
225  // rate_correction FRAC = RATE_FRAC = 12
226  // 2^12 / 2^24 * 5e-2 = 1/81920
227  ahrs_impl.rate_correction.p += -residual.x/82000/inv_weight;
228  ahrs_impl.rate_correction.q += -residual.y/82000/inv_weight;
229  ahrs_impl.rate_correction.r += -residual.z/82000/inv_weight;
230 
231  // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
232  // high_rez_bias = RATE_FRAC+28 = 40
233  // 2^40 / 2^24 * 5e-6 = 1/3.05
234 
235  // ahrs_impl.high_rez_bias.p += residual.x*3;
236  // ahrs_impl.high_rez_bias.q += residual.y*3;
237  // ahrs_impl.high_rez_bias.r += residual.z*3;
238 
239  ahrs_impl.high_rez_bias.p += residual.x/(2*inv_weight);
240  ahrs_impl.high_rez_bias.q += residual.y/(2*inv_weight);
241  ahrs_impl.high_rez_bias.r += residual.z/(2*inv_weight);
242 
243 
244  /* */
246 
247 }
248 
249 void ahrs_update_mag(void) {
250 #if AHRS_MAG_UPDATE_ALL_AXES
252 #else
254 #endif
255 }
256 
257 
258 static inline void ahrs_update_mag_full(void) {
259 
260  struct Int32Vect3 expected_imu;
262 
263  struct Int32Vect3 residual;
264  INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
265 
266  ahrs_impl.rate_correction.p += residual.x/32/16;
267  ahrs_impl.rate_correction.q += residual.y/32/16;
268  ahrs_impl.rate_correction.r += residual.z/32/16;
269 
270 
271  ahrs_impl.high_rez_bias.p -= residual.x/32*1024;
272  ahrs_impl.high_rez_bias.q -= residual.y/32*1024;
273  ahrs_impl.high_rez_bias.r -= residual.z/32*1024;
274 
275 
277 
278 }
279 
280 
281 static inline void ahrs_update_mag_2d(void) {
282 
283  struct Int32Vect3 measured_ltp;
285 
286  struct Int32Vect3 residual_ltp =
287  { 0,
288  0,
289  (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)};
290 
291  struct Int32Vect3 residual_imu;
292  INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
293 
294  // residual_ltp FRAC = 2 * MAG_FRAC = 22
295  // rate_correction FRAC = RATE_FRAC = 12
296  // 2^12 / 2^22 * 2.5 = 1/410
297 
298  // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
299  // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
300  // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
301 
302  ahrs_impl.rate_correction.p += residual_imu.x/16;
303  ahrs_impl.rate_correction.q += residual_imu.y/16;
304  ahrs_impl.rate_correction.r += residual_imu.z/16;
305 
306 
307  // residual_ltp FRAC = 2 * MAG_FRAC = 22
308  // high_rez_bias = RATE_FRAC+28 = 40
309  // 2^40 / 2^22 * 2.5e-3 = 655
310 
311  // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
312  // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
313  // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
314 
315  ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
316  ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
317  ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
318 
319 
321 
322 }
323 
324 void ahrs_update_gps(void) {
325 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
326  if (gps.fix == GPS_FIX_3D) {
329  } else {
331  }
332 #endif
333 
334 #if AHRS_USE_GPS_HEADING && USE_GPS
335  //got a 3d fix,ground speed > 0.5 m/s and course accuracy is better than 10deg
336  if(gps.fix == GPS_FIX_3D &&
337  gps.gspeed >= 500 &&
338  gps.cacc <= RadOfDeg(10*1e7)) {
339 
340  // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
341  int32_t course = gps.course * ((1<<INT32_ANGLE_FRAC) / 1e7);
342 
343  /* the assumption here is that there is no side-slip, so heading=course */
344 
346  ahrs_update_heading(course);
347  }
348  else {
349  /* hard reset the heading if this is the first measurement */
350  ahrs_realign_heading(course);
351  }
352  }
353 #endif
354 }
355 
356 
358 
359  INT32_ANGLE_NORMALIZE(heading);
360 
361  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
362  // we only consider x and y
363  struct Int32Vect2 expected_ltp =
366 
367  int32_t heading_x, heading_y;
368  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
369  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
370 
371  // expected_heading cross measured_heading ??
372  struct Int32Vect3 residual_ltp =
373  { 0,
374  0,
375  (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<<INT32_ANGLE_FRAC)};
376 
377  struct Int32Vect3 residual_imu;
378  INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
379 
380  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
381  // rate_correction FRAC = RATE_FRAC = 12
382  // 2^12 / 2^28 * 4.0 = 1/2^14
383  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
384  ahrs_impl.rate_correction.p += residual_imu.x/4;
385  ahrs_impl.rate_correction.q += residual_imu.y/4;
386  ahrs_impl.rate_correction.r += residual_imu.z/4;
387 
388 
389  /* crude attempt to only update bias if deviation is small
390  * e.g. needed when you only have gps providing heading
391  * and the inital heading is totally different from
392  * the gps course information you get once you have a gps fix.
393  * Otherwise the bias will be falsely "corrected".
394  */
395  int32_t sin_max_angle_deviation;
396  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(5.)));
397  if (ABS(residual_ltp.z) < sin_max_angle_deviation)
398  {
399  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
400  // high_rez_bias = RATE_FRAC+28 = 40
401  // 2^40 / 2^28 * 2.5e-4 = 1
402  ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<INT32_ANGLE_FRAC);
403  ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<INT32_ANGLE_FRAC);
404  ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<INT32_ANGLE_FRAC);
405 
407  }
408 }
409 
411 
412  /* quaternion representing only the heading rotation from ltp to body */
413  struct Int32Quat q_h_new;
414  q_h_new.qx = 0;
415  q_h_new.qy = 0;
416  PPRZ_ITRIG_SIN(q_h_new.qz, heading/2);
417  PPRZ_ITRIG_COS(q_h_new.qi, heading/2);
418 
419  /* quaternion representing current heading only */
420  struct Int32Quat q_h;
422  q_h.qx = 0;
423  q_h.qy = 0;
425 
426  /* quaternion representing rotation from current to new heading */
427  struct Int32Quat q_c;
428  INT32_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
429 
430  /* correct current heading in body frame */
431  struct Int32Quat q;
434 
435  /* compute other representations in body frame */
437 
438  /* compute ltp to imu rotations */
440 
442 }
443 
444 
445 /* Compute ltp to imu rotation in euler angles and rotation matrix representation
446  from the quaternion representation */
447 __attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
448 
449  /* Compute LTP to IMU euler */
451  /* Compute LTP to IMU rotation matrix */
453 
454 }
455 
456 /* Compute ltp to body rotation in euler angles and rotation matrix representation
457  from the quaternion representation */
458 __attribute__ ((always_inline)) static inline void compute_body_euler_and_rmat_from_quat(void) {
459 
460  /* Compute LTP to body euler */
462  /* Compute LTP to body rotation matrix */
464 
465 }
466 
467 __attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
468 
469  /* Compute LTP to BODY quaternion */
471  /* Compute LTP to BODY rotation matrix */
473  /* compute LTP to BODY eulers */
475  /* compute body rates */
477 
478 }
479 
480 __attribute__ ((always_inline)) static inline void compute_imu_orientation(void) {
481 
482  /* Compute LTP to IMU quaternion */
484  /* Compute LTP to IMU rotation matrix */
486  /* compute LTP to IMU eulers */
488  /* compute IMU rates */
490 
491 }
492 
493 
494 #ifdef AHRS_UPDATE_FW_ESTIMATOR
495 // TODO use ahrs result directly
496 #include "estimator.h"
497 // remotely settable
498 #ifndef INS_ROLL_NEUTRAL_DEFAULT
499 #define INS_ROLL_NEUTRAL_DEFAULT 0
500 #endif
501 #ifndef INS_PITCH_NEUTRAL_DEFAULT
502 #define INS_PITCH_NEUTRAL_DEFAULT 0
503 #endif
504 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
505 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
506 void ahrs_update_fw_estimator(void)
507 {
508  struct FloatEulers att;
509  // export results to estimator
511 
512  estimator_phi = att.phi - ins_roll_neutral;
513  estimator_theta = att.theta - ins_pitch_neutral;
514  estimator_psi = att.psi;
515 
516  struct FloatRates rates;
518  estimator_p = rates.p;
519  estimator_q = rates.q;
520  estimator_r = rates.r;
521 
522 }
523 #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
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:162
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 COMPUTATION_FRAC
#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
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
void ahrs_update_fw_estimator(void)
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:120
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)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:113
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 VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:169
#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
struct Int32Vect3 mag_h
#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)