Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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 */
43static 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
59void 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
66 ahrs_madgwick.is_aligned = true;
67}
68
69void 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 gyro
82 RATES_DIFF(ahrs_madgwick.rates, *gyro, ahrs_madgwick.bias);
83
84 // Rate of change of quaternion from gyroscope
86
87 // compute accel norm
88 float norm = float_vect3_norm(&ahrs_madgwick.accel);
89 // Compute feedback only if accelerometer measurement valid
90 // (avoids NaN in accelerometer normalisation)
91 if (norm > 0.01f) {
92 // Normalise accelerometer measurement
93 // direction of accel is inverted to comply with filter original frame
94 struct FloatVect3 a;
95 VECT3_SDIV(a, ahrs_madgwick.accel, -norm);
96
97 // Auxiliary variables to avoid repeated arithmetic
98 const float q1 = ahrs_madgwick.quat.qx;
99 const float q2 = ahrs_madgwick.quat.qy;
100 const float q3 = ahrs_madgwick.quat.qz;
101 const float _2q0 = 2.0f * ahrs_madgwick.quat.qi;
102 const float _2q1 = 2.0f * ahrs_madgwick.quat.qx;
103 const float _2q2 = 2.0f * ahrs_madgwick.quat.qy;
104 const float _2q3 = 2.0f * ahrs_madgwick.quat.qz;
105 const float _4q0 = 4.0f * ahrs_madgwick.quat.qi;
106 const float _4q1 = 4.0f * ahrs_madgwick.quat.qx;
107 const float _4q2 = 4.0f * ahrs_madgwick.quat.qy;
108 const float _8q1 = 8.0f * ahrs_madgwick.quat.qx;
109 const float _8q2 = 8.0f * ahrs_madgwick.quat.qy;
110 const float q0q0 = ahrs_madgwick.quat.qi * ahrs_madgwick.quat.qi;
111 const float q1q1 = ahrs_madgwick.quat.qx * ahrs_madgwick.quat.qx;
112 const float q2q2 = ahrs_madgwick.quat.qy * ahrs_madgwick.quat.qy;
113 const float q3q3 = ahrs_madgwick.quat.qz * ahrs_madgwick.quat.qz;
114
115 // Gradient decent algorithm corrective step
116 const float s0 = _4q0 * q2q2 + _2q2 * a.x + _4q0 * q1q1 - _2q1 * a.y;
117 const float s1 = _4q1 * q3q3 - _2q3 * a.x + 4.0f * q0q0 * q1 - _2q0 * a.y - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * a.z;
118 const float s2 = 4.0f * q0q0 * q2 + _2q0 * a.x + _4q2 * q3q3 - _2q3 * a.y - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * a.z;
119 const float s3 = 4.0f * q1q1 * q3 - _2q1 * a.x + 4.0f * q2q2 * q3 - _2q2 * a.y;
120 const float beta_inv_grad_norm = AHRS_MADGWICK_BETA / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
121
122 // Apply feedback step
123 qdot.qi -= s0 * beta_inv_grad_norm;
124 qdot.qx -= s1 * beta_inv_grad_norm;
125 qdot.qy -= s2 * beta_inv_grad_norm;
126 qdot.qz -= s3 * beta_inv_grad_norm;
127 }
128
129 // Integrate rate of change of quaternion to yield quaternion
130 ahrs_madgwick.quat.qi += qdot.qi * dt;
131 ahrs_madgwick.quat.qx += qdot.qx * dt;
132 ahrs_madgwick.quat.qy += qdot.qy * dt;
133 ahrs_madgwick.quat.qz += qdot.qz * dt;
134
135 // Normalise quaternion
137}
138
140{
141 ahrs_madgwick.accel = *accel;
142}
Utility functions for floating point AHRS implementations.
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 AhrsMadgwick ahrs_madgwick
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
static void init_state(void)
void ahrs_madgwick_init(void)
#define AHRS_MADGWICK_BETA
AHRS using Madgwick implementation.
Madgwick filter structure.
static void float_quat_normalize(struct FloatQuat *q)
static float float_vect3_norm(const struct FloatVect3 *v)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_RATES_ZERO(_r)
void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Roation quaternion.
angular rates
#define VECT3_SDIV(_vo, _vi, _s)
#define RATES_DIFF(_c, _a, _b)
Paparazzi floating point algebra.