GCC Code Coverage Report

Directory: ./
File: src/filters/madgwickahrs.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 97 111 87.4%
Branches: 63 162 38.9%
Line Branch Exec Source
1 //=========================================================================
2 //
3 // Implementation of Madgwick's IMU and AHRS algorithms.
4 // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
5 //
6 // Date Author Notes
7 // 29/09/2011 SOH Madgwick Initial release
8 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
9 // 11/05/2017 T Flayols Make it a dynamic-graph entity
10 //
11 //=========================================================================
12
13 #include <dynamic-graph/all-commands.h>
14 #include <dynamic-graph/factory.h>
15
16 #include <sot/core/debug.hh>
17 #include <sot/core/madgwickahrs.hh>
18 #include <sot/core/stop-watch.hh>
19
20 namespace dynamicgraph {
21 namespace sot {
22 namespace dg = ::dynamicgraph;
23 using namespace dg;
24 using namespace dg::command;
25 using namespace std;
26
27 typedef Vector6d Vector6;
28
29 #define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
30
31 #define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
32 #define OUTPUT_SIGNALS m_imu_quatSOUT
33
34 /// Define EntityClassName here rather than in the header file
35 /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
36 typedef MadgwickAHRS EntityClassName;
37
38 /* --- DG FACTORY ---------------------------------------------------- */
39 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS, "MadgwickAHRS");
40
41 /* ------------------------------------------------------------------- */
42 /* --- CONSTRUCTION -------------------------------------------------- */
43 /* ------------------------------------------------------------------- */
44 1 MadgwickAHRS::MadgwickAHRS(const std::string &name)
45 : Entity(name),
46
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1 CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
47
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
48
10/20
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
 1 CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector,
49 m_gyroscopeSIN << m_accelerometerSIN),
50 1 m_initSucceeded(false),
51 1 m_beta(betaDef),
52 1 m_q0(1.0),
53 1 m_q1(0.0),
54 1 m_q2(0.0),
55 1 m_q3(0.0),
56 1 m_sampleFreq(512.0) {
57
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
58
59 /* Commands. */
60
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("init",
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(*this, &MadgwickAHRS::init,
62
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Initialize the entity.",
63 "Timestep in seconds (double)")));
64
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("getBeta",
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeDirectGetter(*this, &m_beta,
66
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docDirectGetter("Beta parameter", "double")));
67
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setBeta",
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(
69 *this, &MadgwickAHRS::set_beta,
70
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Set the filter parameter beta", "double")));
71
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("set_imu_quat",
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(
73 *this, &MadgwickAHRS::set_imu_quat,
74
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Set the quaternion as [w,x,y,z]", "vector")));
75 1 }
76
77 1 void MadgwickAHRS::init(const double &dt) {
78
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 1 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
79 1 m_sampleFreq = 1.0 / dt;
80 1 m_initSucceeded = true;
81 }
82
83 1 void MadgwickAHRS::set_beta(const double &beta) {
84
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (beta < 0.0 || beta > 1.0)
85 ✗ return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
86 1 m_beta = beta;
87 }
88
89 ✗ void MadgwickAHRS::set_imu_quat(const dynamicgraph::Vector &imu_quat) {
90 ✗ assert(imu_quat.size() == 4);
91 ✗ m_q0 = imu_quat[0];
92 ✗ m_q1 = imu_quat[1];
93 ✗ m_q2 = imu_quat[2];
94 ✗ m_q3 = imu_quat[3];
95 }
96
97 /* ------------------------------------------------------------------- */
98 /* --- SIGNALS ------------------------------------------------------- */
99 /* ------------------------------------------------------------------- */
100
101 1 DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector) {
102
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (!m_initSucceeded) {
103 ✗ SEND_WARNING_STREAM_MSG(
104 "Cannot compute signal imu_quat before initialization!");
105 ✗ return s;
106 }
107 1 const dynamicgraph::Vector &accelerometer = m_accelerometerSIN(iter);
108 1 const dynamicgraph::Vector &gyroscope = m_gyroscopeSIN(iter);
109
110
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
111 {
112 // Update state with new measurment
113 1 madgwickAHRSupdateIMU(gyroscope(0), gyroscope(1), gyroscope(2),
114 1 accelerometer(0), accelerometer(1), accelerometer(2));
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (s.size() != 4) s.resize(4);
116 1 s(0) = m_q0;
117 1 s(1) = m_q1;
118 1 s(2) = m_q2;
119 1 s(3) = m_q3;
120 }
121
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
122
123 1 return s;
124 }
125
126 /* --- COMMANDS ------------------------------------------------------ */
127
128 /* ------------------------------------------------------------------- */
129 // ************************ PROTECTED MEMBER METHODS ********************
130 /* ------------------------------------------------------------------- */
131
132 // Fast inverse square-root
133 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
134 3 double MadgwickAHRS::invSqrt(double x) {
135 /*
136 float halfx = 0.5f * x;
137 float y = x;
138 long i = *(long*)&y;
139 i = 0x5f3759df - (i>>1);
140 y = *(float*)&i;
141 y = y * (1.5f - (halfx * y * y));
142 return y;*/
143 3 return (1.0 / sqrt(x)); // we're not in the 70's
144 }
145
146 // IMU algorithm update
147 1 void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz,
148 double ax, double ay, double az) {
149 double recipNorm;
150 double s0, s1, s2, s3;
151 double qDot1, qDot2, qDot3, qDot4;
152 double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2, o8q1, o8q2;
153 double q0q0, q1q1, q2q2, q3q3;
154
155 // Rate of change of quaternion from gyroscope
156 1 qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
157 1 qDot2 = 0.5 * (m_q0 * gx + m_q2 * gz - m_q3 * gy);
158 1 qDot3 = 0.5 * (m_q0 * gy - m_q1 * gz + m_q3 * gx);
159 1 qDot4 = 0.5 * (m_q0 * gz + m_q1 * gy - m_q2 * gx);
160
161 // Compute feedback only if accelerometer measurement valid
162 // (avoids NaN in accelerometer normalisation)
163
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1 if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
164 // Normalise accelerometer measurement
165 1 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
166 1 ax *= recipNorm;
167 1 ay *= recipNorm;
168 1 az *= recipNorm;
169
170 // Auxiliary variables to avoid repeated arithmetic
171 1 o2q0 = 2.0 * m_q0;
172 1 o2q1 = 2.0 * m_q1;
173 1 o2q2 = 2.0 * m_q2;
174 1 o2q3 = 2.0 * m_q3;
175 1 o4q0 = 4.0 * m_q0;
176 1 o4q1 = 4.0 * m_q1;
177 1 o4q2 = 4.0 * m_q2;
178 1 o8q1 = 8.0 * m_q1;
179 1 o8q2 = 8.0 * m_q2;
180 1 q0q0 = m_q0 * m_q0;
181 1 q1q1 = m_q1 * m_q1;
182 1 q2q2 = m_q2 * m_q2;
183 1 q3q3 = m_q3 * m_q3;
184
185 // Gradient decent algorithm corrective step
186 1 s0 = o4q0 * q2q2 + o2q2 * ax + o4q0 * q1q1 - o2q1 * ay;
187 1 s1 = o4q1 * q3q3 - o2q3 * ax + 4.0 * q0q0 * m_q1 - o2q0 * ay - o4q1 +
188 1 o8q1 * q1q1 + o8q1 * q2q2 + o4q1 * az;
189 1 s2 = 4.0 * q0q0 * m_q2 + o2q0 * ax + o4q2 * q3q3 - o2q3 * ay - o4q2 +
190 1 o8q2 * q1q1 + o8q2 * q2q2 + o4q2 * az;
191 1 s3 = 4.0 * q1q1 * m_q3 - o2q1 * ax + 4.0 * q2q2 * m_q3 - o2q2 * ay;
192
2/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 if (!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0))) {
193 // normalise step magnitude
194 1 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
195 1 s0 *= recipNorm;
196 1 s1 *= recipNorm;
197 1 s2 *= recipNorm;
198 1 s3 *= recipNorm;
199
200 // Apply feedback step
201 1 qDot1 -= m_beta * s0;
202 1 qDot2 -= m_beta * s1;
203 1 qDot3 -= m_beta * s2;
204 1 qDot4 -= m_beta * s3;
205 }
206 }
207
208 // Integrate rate of change of quaternion to yield quaternion
209 1 m_q0 += qDot1 * (1.0 / m_sampleFreq);
210 1 m_q1 += qDot2 * (1.0 / m_sampleFreq);
211 1 m_q2 += qDot3 * (1.0 / m_sampleFreq);
212 1 m_q3 += qDot4 * (1.0 / m_sampleFreq);
213
214 // Normalise quaternion
215 1 recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
216 1 m_q0 *= recipNorm;
217 1 m_q1 *= recipNorm;
218 1 m_q2 *= recipNorm;
219 1 m_q3 *= recipNorm;
220 1 }
221
222 /* ------------------------------------------------------------------- */
223 /* --- ENTITY -------------------------------------------------------- */
224 /* ------------------------------------------------------------------- */
225
226 ✗ void MadgwickAHRS::display(std::ostream &os) const {
227 ✗ os << "MadgwickAHRS " << getName();
228 try {
229 ✗ getProfiler().report_all(3, os);
230 ✗ } catch (ExceptionSignal e) {
231 }
232 }
233 } // namespace sot
234 } // namespace dynamicgraph
235