Paparazzi UAS  v5.15_devel-132-gbbc6108
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
Definition: ahrs_madgwick.c:59
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_madgwick.c:69
static void init_state(void)
Definition: ahrs_madgwick.c:43
struct FloatRates bias
Gyro bias (from alignment)
Definition: ahrs_madgwick.h:41
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Utility functions for floating point AHRS implementations.
#define FLOAT_RATES_ZERO(_r)
struct AhrsMadgwick ahrs_madgwick
Definition: ahrs_madgwick.c:39
bool is_aligned
aligned flag
Definition: ahrs_madgwick.h:45
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
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.
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_madgwick.h:39
Roation quaternion.
static float float_vect3_norm(struct FloatVect3 *v)
#define FLOAT_VECT3_ZERO(_v)
Paparazzi floating point algebra.
struct FloatRates rates
Measured gyro rates.
Definition: ahrs_madgwick.h:40
#define AHRS_MADGWICK_BETA
Definition: ahrs_madgwick.c:35
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
static void float_quat_normalize(struct FloatQuat *q)
void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
bool reset
flag to request reset/reinit the filter
Definition: ahrs_madgwick.h:44
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
struct FloatVect3 accel
Measured accelerometers.
Definition: ahrs_madgwick.h:42
rotation matrix
AHRS using Madgwick implementation.
void ahrs_madgwick_init(void)
Definition: ahrs_madgwick.c:50
angular rates
Madgwick filter structure.
Definition: ahrs_madgwick.h:38
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_madgwick.h:43