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_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 
34 
35 #include "state.h"
36 
37 #include "subsystems/imu.h"
38 #if USE_GPS
39 #include "subsystems/gps.h"
40 #endif
41 #include "math/pprz_trig_int.h"
42 #include "math/pprz_algebra_int.h"
43 
44 #include "generated/airframe.h"
45 
46 //#include "../../test/pprz_algebra_print.h"
47 
48 static inline void ahrs_update_mag_full(void);
49 static inline void ahrs_update_mag_2d(void);
50 
51 #ifdef AHRS_MAG_UPDATE_YAW_ONLY
52 #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."
53 #endif
54 
55 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
56 #warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
57 #endif
58 
59 #ifndef AHRS_PROPAGATE_FREQUENCY
60 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
61 #endif
62 
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
76 
77 static inline void set_body_state_from_quat(void);
78 
79 
80 void ahrs_init(void) {
81 
85 
86  /* set ltp_to_imu so that body is zero */
89 
93 
94 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
96 #else
98 #endif
99 
100 #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
102 #else
104 #endif
105 
107 
108 }
109 
110 void ahrs_align(void) {
111 
112 #if USE_MAGNETOMETER
113  /* Compute an initial orientation from accel and mag directly as quaternion */
116 #else
117  /* Compute an initial orientation from accel and just set heading to zero */
120 #endif
121 
123 
124  /* Use low passed gyro value as initial bias */
128 
130 }
131 
132 
133 
134 /*
135  *
136  *
137  *
138  */
139 void ahrs_propagate(void) {
140 
141  /* unbias gyro */
142  struct Int32Rates omega;
144 
145  /* low pass rate */
146 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
148  RATES_ADD(ahrs_impl.imu_rate, omega);
150 #else
151  RATES_COPY(ahrs_impl.imu_rate, omega);
152 #endif
153 
154  /* add correction */
156  /* and zeros it */
158 
159  /* integrate quaternion */
162 
164 
165 }
166 
167 
168 
169 
170 void ahrs_update_accel(void) {
171 
172  // c2 = ltp z-axis in imu-frame
173  struct Int32RMat ltp_to_imu_rmat;
174  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
175  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2),
176  RMAT_ELMT(ltp_to_imu_rmat, 1,2),
177  RMAT_ELMT(ltp_to_imu_rmat, 2,2)};
178  struct Int32Vect3 residual;
179 
180  struct Int32Vect3 pseudo_gravity_measurement;
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 
191  // FIXME: check overflows !
192 #define COMPUTATION_FRAC 16
193 
194  const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
195  struct Int32Vect3 acc_c_body;
196  VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body);
198 
199  /* convert centrifucal acceleration from body to imu frame */
200  struct Int32Vect3 acc_c_imu;
201  INT32_RMAT_VMULT(acc_c_imu, imu.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  INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
205  } else {
206  VECT3_COPY(pseudo_gravity_measurement, imu.accel);
207  }
208 
209  /* compute the residual of the pseudo gravity vector in imu frame */
210  INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
211 
212 
213  int32_t inv_weight;
215  /* heuristic on acceleration norm */
216 
217  /* FIR filtered pseudo_gravity_measurement */
218  static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
219  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7);
220  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
221  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8);
222 
223  int32_t acc_norm;
224  INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement);
225  const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm);
226  inv_weight = Chop(50*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 50);
227  }
228  else {
229  inv_weight = 1;
230  }
231 
232  // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
233  // rate_correction FRAC = RATE_FRAC = 12
234  // 2^12 / 2^24 * 5e-2 = 1/81920
235  ahrs_impl.rate_correction.p += -residual.x/82000/inv_weight;
236  ahrs_impl.rate_correction.q += -residual.y/82000/inv_weight;
237  ahrs_impl.rate_correction.r += -residual.z/82000/inv_weight;
238 
239  // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
240  // high_rez_bias = RATE_FRAC+28 = 40
241  // 2^40 / 2^24 * 5e-6 = 1/3.05
242 
243  // ahrs_impl.high_rez_bias.p += residual.x*3;
244  // ahrs_impl.high_rez_bias.q += residual.y*3;
245  // ahrs_impl.high_rez_bias.r += residual.z*3;
246 
247  ahrs_impl.high_rez_bias.p += residual.x/(2*inv_weight);
248  ahrs_impl.high_rez_bias.q += residual.y/(2*inv_weight);
249  ahrs_impl.high_rez_bias.r += residual.z/(2*inv_weight);
250 
251 
252  /* */
254 
255 }
256 
257 void ahrs_update_mag(void) {
258 #if AHRS_MAG_UPDATE_ALL_AXES
260 #else
262 #endif
263 }
264 
265 
266 static inline void ahrs_update_mag_full(void) {
267 
268  struct Int32RMat ltp_to_imu_rmat;
269  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
270 
271  struct Int32Vect3 expected_imu;
272  INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, ahrs_impl.mag_h);
273 
274  struct Int32Vect3 residual;
275  INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
276 
277  ahrs_impl.rate_correction.p += residual.x/32/16;
278  ahrs_impl.rate_correction.q += residual.y/32/16;
279  ahrs_impl.rate_correction.r += residual.z/32/16;
280 
281 
282  ahrs_impl.high_rez_bias.p -= residual.x/32*1024;
283  ahrs_impl.high_rez_bias.q -= residual.y/32*1024;
284  ahrs_impl.high_rez_bias.r -= residual.z/32*1024;
285 
286 
288 
289 }
290 
291 
292 static inline void ahrs_update_mag_2d(void) {
293 
294  struct Int32RMat ltp_to_imu_rmat;
295  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
296 
297  struct Int32Vect3 measured_ltp;
298  INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag);
299 
300  struct Int32Vect3 residual_ltp =
301  { 0,
302  0,
303  (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)};
304 
305  struct Int32Vect3 residual_imu;
306  INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp);
307 
308  // residual_ltp FRAC = 2 * MAG_FRAC = 22
309  // rate_correction FRAC = RATE_FRAC = 12
310  // 2^12 / 2^22 * 2.5 = 1/410
311 
312  // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
313  // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
314  // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
315 
316  ahrs_impl.rate_correction.p += residual_imu.x/16;
317  ahrs_impl.rate_correction.q += residual_imu.y/16;
318  ahrs_impl.rate_correction.r += residual_imu.z/16;
319 
320 
321  // residual_ltp FRAC = 2 * MAG_FRAC = 22
322  // high_rez_bias = RATE_FRAC+28 = 40
323  // 2^40 / 2^22 * 2.5e-3 = 655
324 
325  // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
326  // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
327  // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
328 
329  ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
330  ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
331  ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
332 
333 
335 
336 }
337 
338 void ahrs_update_gps(void) {
339 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
340  if (gps.fix == GPS_FIX_3D) {
343  } else {
345  }
346 #endif
347 
348 #if AHRS_USE_GPS_HEADING && USE_GPS
349  //got a 3d fix,ground speed > 0.5 m/s and course accuracy is better than 10deg
350  if(gps.fix == GPS_FIX_3D &&
351  gps.gspeed >= 500 &&
352  gps.cacc <= RadOfDeg(10*1e7)) {
353 
354  // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
355  int32_t course = gps.course * ((1<<INT32_ANGLE_FRAC) / 1e7);
356 
357  /* the assumption here is that there is no side-slip, so heading=course */
358 
360  ahrs_update_heading(course);
361  }
362  else {
363  /* hard reset the heading if this is the first measurement */
364  ahrs_realign_heading(course);
365  }
366  }
367 #endif
368 }
369 
370 
372 
373  INT32_ANGLE_NORMALIZE(heading);
374 
375  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
376  // we only consider x and y
377  struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i();
378  struct Int32Vect2 expected_ltp =
379  { RMAT_ELMT((*ltp_to_body_rmat), 0, 0),
380  RMAT_ELMT((*ltp_to_body_rmat), 0, 1) };
381 
382  int32_t heading_x, heading_y;
383  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
384  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
385 
386  // expected_heading cross measured_heading ??
387  struct Int32Vect3 residual_ltp =
388  { 0,
389  0,
390  (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<<INT32_ANGLE_FRAC)};
391 
392  struct Int32Vect3 residual_imu;
393  struct Int32RMat ltp_to_imu_rmat;
394  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
395  INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp);
396 
397  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
398  // rate_correction FRAC = RATE_FRAC = 12
399  // 2^12 / 2^28 * 4.0 = 1/2^14
400  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
401  ahrs_impl.rate_correction.p += residual_imu.x/4;
402  ahrs_impl.rate_correction.q += residual_imu.y/4;
403  ahrs_impl.rate_correction.r += residual_imu.z/4;
404 
405 
406  /* crude attempt to only update bias if deviation is small
407  * e.g. needed when you only have gps providing heading
408  * and the inital heading is totally different from
409  * the gps course information you get once you have a gps fix.
410  * Otherwise the bias will be falsely "corrected".
411  */
412  int32_t sin_max_angle_deviation;
413  PPRZ_ITRIG_SIN(sin_max_angle_deviation, TRIG_BFP_OF_REAL(RadOfDeg(5.)));
414  if (ABS(residual_ltp.z) < sin_max_angle_deviation)
415  {
416  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
417  // high_rez_bias = RATE_FRAC+28 = 40
418  // 2^40 / 2^28 * 2.5e-4 = 1
419  ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<INT32_ANGLE_FRAC);
420  ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<INT32_ANGLE_FRAC);
421  ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<INT32_ANGLE_FRAC);
422 
424  }
425 }
426 
428 
429  struct Int32Quat ltp_to_body_quat = *stateGetNedToBodyQuat_i();
430 
431  /* quaternion representing only the heading rotation from ltp to body */
432  struct Int32Quat q_h_new;
433  q_h_new.qx = 0;
434  q_h_new.qy = 0;
435  PPRZ_ITRIG_SIN(q_h_new.qz, heading/2);
436  PPRZ_ITRIG_COS(q_h_new.qi, heading/2);
437 
438  /* quaternion representing current heading only */
439  struct Int32Quat q_h;
440  QUAT_COPY(q_h, ltp_to_body_quat);
441  q_h.qx = 0;
442  q_h.qy = 0;
444 
445  /* quaternion representing rotation from current to new heading */
446  struct Int32Quat q_c;
447  INT32_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
448 
449  /* correct current heading in body frame */
450  struct Int32Quat q;
451  INT32_QUAT_COMP_NORM_SHORTEST(q, q_c, ltp_to_body_quat);
452  QUAT_COPY(ltp_to_body_quat, q);
453 
454  /* compute ltp to imu rotations */
456 
457  /* Set state */
458  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
459 
461 }
462 
463 
464 /* Rotate angles and rates from imu to body frame and set state */
465 __attribute__ ((always_inline)) static inline void set_body_state_from_quat(void) {
466  /* Compute LTP to BODY quaternion */
467  struct Int32Quat ltp_to_body_quat;
469  /* Set state */
470 #ifdef AHRS_UPDATE_FW_ESTIMATOR
471  struct Int32Eulers neutrals_to_body_eulers = {
474  0 };
475  struct Int32Quat neutrals_to_body_quat, ltp_to_neutrals_quat;
476  INT32_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
477  INT32_QUAT_NORMALIZE(neutrals_to_body_quat);
478  INT32_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
479  stateSetNedToBodyQuat_i(&ltp_to_neutrals_quat);
480 #else
481  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
482 #endif
483 
484  /* compute body rates */
485  struct Int32Rates body_rate;
487  /* Set state */
488  stateSetBodyRates_i(&body_rate);
489 }
490 
491 
Quaternion complementary filter (fixed-point).
Interface to align the AHRS via low-passed measurements at startup.
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:336
bool_t use_gravity_heuristic
static void ahrs_update_mag_2d(void)
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
void ahrs_realign_heading(int32_t heading)
Hard reset yaw to a heading.
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
#define ANGLE_BFP_OF_REAL(_af)
int32_t p
in rad/s^2 with INT32_RATE_FRAC
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
#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:160
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
void ahrs_init(void)
AHRS initialization.
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
#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:75
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
struct Int32Rates imu_rate
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
void ahrs_align(void)
Aligns the AHRS.
#define INT_RATES_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
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
#define INT32_VECT3_NORM(n, v)
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1002
#define INT_RATES_RSHIFT(_o, _i, _r)
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:343
#define FALSE
Definition: imu_chimu.h:141
#define COMPUTATION_FRAC
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:350
struct Int32Rates rate_correction
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
static void ahrs_update_mag_full(void)
#define INT32_QUAT_OF_EULERS(_q, _e)
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1071
float heading
Definition: ahrs_infrared.c:40
#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:35
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
#define TRIG_BFP_OF_REAL(_tf)
#define INT32_VECT3_RSHIFT(_o, _i, _r)
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
#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:308
struct Int64Rates high_rez_bias
struct Int32Quat ltp_to_imu_quat
bool_t correct_gravity
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:518
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1055
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
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define AHRS_H_Y
signed long int32_t
Definition: types.h:19
int64_t p
in rad/s^2 with INT32_RATE_FRAC
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:970
Utility functions for fixed point AHRS implementations.
#define AHRS_UNINIT
Definition: ahrs.h:35
#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:97
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:118
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
#define INT32_RMAT_OF_QUAT(_rm, _q)
#define AHRS_H_X
API to get/set the generic vehicle states.
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:476
struct AhrsIntCmpl ahrs_impl
int32_t q
in rad/s^2 with INT32_RATE_FRAC
rotation matrix
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
int64_t q
in rad/s^2 with INT32_RATE_FRAC
#define INT32_SPEED_FRAC
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)
Update AHRS state with GPS measurements.
int32_t ltp_vel_norm
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:167
#define INT_RATES_LSHIFT(_o, _i, _r)
#define INT32_ACCEL_FRAC
#define SPEED_BFP_OF_REAL(_af)
struct GpsState gps
global GPS state
Definition: gps.c:33
struct Int64Quat high_rez_quat
static void set_body_state_from_quat(void)
void ahrs_propagate(void)
Propagation.
#define AHRS_RUNNING
Definition: ahrs.h:36
struct Int32Vect3 mag_h
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
euler angles