Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_madgwick.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
30 #include "generated/airframe.h"
33 
34 #ifndef AHRS_MADGWICK_BETA
35 #define AHRS_MADGWICK_BETA 0.1f // 2 * proportional gain
36 #endif
37 
38 
40 
41 
42 /* init state and measurements */
43 static inline void init_state(void)
44 {
48 }
49 
51 {
52  // init state and measurements
53  init_state();
54 
55  ahrs_madgwick.is_aligned = false;
56  ahrs_madgwick.reset = false;
57 }
58 
59 void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
60 {
61  /* Compute an initial orientation from accel directly as quaternion */
63  /* use average gyro as initial value for bias */
64  ahrs_madgwick.bias = *lp_gyro;
65  // ins and ahrs are now running
67 }
68 
69 void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt)
70 {
71  struct FloatQuat qdot;
72 
73  // realign all the filter if needed
74  // a complete init cycle is required
75  if (ahrs_madgwick.reset) {
76  ahrs_madgwick.reset = false;
77  ahrs_madgwick.is_aligned = false;
78  init_state();
79  }
80 
81  // unbias and rotate gyro
82  struct FloatRates gyro_unbiased;
83  RATES_DIFF(gyro_unbiased, *gyro, ahrs_madgwick.bias);
84  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_madgwick.body_to_imu);
85  float_rmat_transp_ratemult(&ahrs_madgwick.rates, body_to_imu_rmat, &gyro_unbiased);
86 
87  // Rate of change of quaternion from gyroscope
89 
90  // compute accel norm
91  float norm = float_vect3_norm(&ahrs_madgwick.accel);
92  // Compute feedback only if accelerometer measurement valid
93  // (avoids NaN in accelerometer normalisation)
94  if (norm > 0.01f) {
95  // Normalise accelerometer measurement
96  // direction of accel is inverted to comply with filter original frame
97  struct FloatVect3 a;
98  VECT3_SDIV(a, ahrs_madgwick.accel, -norm);
99 
100  // Auxiliary variables to avoid repeated arithmetic
101  const float q1 = ahrs_madgwick.quat.qx;
102  const float q2 = ahrs_madgwick.quat.qy;
103  const float q3 = ahrs_madgwick.quat.qz;
104  const float _2q0 = 2.0f * ahrs_madgwick.quat.qi;
105  const float _2q1 = 2.0f * ahrs_madgwick.quat.qx;
106  const float _2q2 = 2.0f * ahrs_madgwick.quat.qy;
107  const float _2q3 = 2.0f * ahrs_madgwick.quat.qz;
108  const float _4q0 = 4.0f * ahrs_madgwick.quat.qi;
109  const float _4q1 = 4.0f * ahrs_madgwick.quat.qx;
110  const float _4q2 = 4.0f * ahrs_madgwick.quat.qy;
111  const float _8q1 = 8.0f * ahrs_madgwick.quat.qx;
112  const float _8q2 = 8.0f * ahrs_madgwick.quat.qy;
113  const float q0q0 = ahrs_madgwick.quat.qi * ahrs_madgwick.quat.qi;
114  const float q1q1 = ahrs_madgwick.quat.qx * ahrs_madgwick.quat.qx;
115  const float q2q2 = ahrs_madgwick.quat.qy * ahrs_madgwick.quat.qy;
116  const float q3q3 = ahrs_madgwick.quat.qz * ahrs_madgwick.quat.qz;
117 
118  // Gradient decent algorithm corrective step
119  const float s0 = _4q0 * q2q2 + _2q2 * a.x + _4q0 * q1q1 - _2q1 * a.y;
120  const float s1 = _4q1 * q3q3 - _2q3 * a.x + 4.0f * q0q0 * q1 - _2q0 * a.y - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * a.z;
121  const float s2 = 4.0f * q0q0 * q2 + _2q0 * a.x + _4q2 * q3q3 - _2q3 * a.y - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * a.z;
122  const float s3 = 4.0f * q1q1 * q3 - _2q1 * a.x + 4.0f * q2q2 * q3 - _2q2 * a.y;
123  const float beta_inv_grad_norm = AHRS_MADGWICK_BETA / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
124 
125  // Apply feedback step
126  qdot.qi -= s0 * beta_inv_grad_norm;
127  qdot.qx -= s1 * beta_inv_grad_norm;
128  qdot.qy -= s2 * beta_inv_grad_norm;
129  qdot.qz -= s3 * beta_inv_grad_norm;
130  }
131 
132  // Integrate rate of change of quaternion to yield quaternion
133  ahrs_madgwick.quat.qi += qdot.qi * dt;
134  ahrs_madgwick.quat.qx += qdot.qx * dt;
135  ahrs_madgwick.quat.qy += qdot.qy * dt;
136  ahrs_madgwick.quat.qz += qdot.qz * dt;
137 
138  // Normalise quaternion
140 }
141 
143 {
144  ahrs_madgwick.accel = *accel;
145 }
146 
148 {
150 
151  if (!ahrs_madgwick.is_aligned) {
152  /* Set ltp_to_imu so that body is zero */
153  ahrs_madgwick.quat = *q_b2i;
154  }
155 }
156 
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
ahrs_madgwick_init
void ahrs_madgwick_init(void)
Definition: ahrs_madgwick.c:50
ahrs_madgwick_set_body_to_imu_quat
void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_madgwick.c:147
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
ahrs_madgwick_update_accel
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
Definition: ahrs_madgwick.c:142
AhrsMadgwick::bias
struct FloatRates bias
Gyro bias (from alignment)
Definition: ahrs_madgwick.h:41
VECT3_SDIV
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
ahrs_float_utils.h
pprz_algebra_float.h
Paparazzi floating point algebra.
ahrs_madgwick
struct AhrsMadgwick ahrs_madgwick
Definition: ahrs_madgwick.c:39
logger_uart_parse.s2
s2
Definition: logger_uart_parse.py:9
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
AhrsMadgwick::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_madgwick.h:39
init_state
static void init_state(void)
Definition: ahrs_madgwick.c:43
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
AhrsMadgwick::is_aligned
bool is_aligned
aligned flag
Definition: ahrs_madgwick.h:45
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
ahrs_madgwick.h
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
ahrs_madgwick_align
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
Definition: ahrs_madgwick.c:59
AhrsMadgwick::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_madgwick.h:43
AhrsMadgwick::reset
bool reset
flag to request reset/reinit the filter
Definition: ahrs_madgwick.h:44
ahrs_float_get_quat_from_accel
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
Definition: ahrs_float_utils.h:61
float_quat_derivative
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_float.c:450
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
AhrsMadgwick::rates
struct FloatRates rates
Measured gyro rates.
Definition: ahrs_madgwick.h:40
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
AHRS_MADGWICK_BETA
#define AHRS_MADGWICK_BETA
Definition: ahrs_madgwick.c:35
AhrsMadgwick::accel
struct FloatVect3 accel
Measured accelerometers.
Definition: ahrs_madgwick.h:42
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_float.c:160
ahrs_madgwick_propagate
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_madgwick.c:69
AhrsMadgwick
Madgwick filter structure.
Definition: ahrs_madgwick.h:38
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
logger_uart_parse.s1
s1
Definition: logger_uart_parse.py:9
logger_uart_parse.s3
s3
Definition: logger_uart_parse.py:9
FloatRates
angular rates
Definition: pprz_algebra_float.h:93