| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector), |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector), |
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector, |
49 |
m_gyroscopeSIN << m_accelerometerSIN), |
||
50 |
m_initSucceeded(false), |
||
51 |
m_beta(betaDef), |
||
52 |
m_q0(1.0), |
||
53 |
m_q1(0.0), |
||
54 |
m_q2(0.0), |
||
55 |
m_q3(0.0), |
||
56 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
m_sampleFreq(512.0) { |
57 |
✓✗✓✗ ✓✗ |
1 |
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
58 |
|||
59 |
/* Commands. */ |
||
60 |
✓✗✓✗ |
1 |
addCommand("init", |
61 |
✓✗ | 1 |
makeCommandVoid1(*this, &MadgwickAHRS::init, |
62 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Initialize the entity.", |
63 |
"Timestep in seconds (double)"))); |
||
64 |
✓✗✓✗ |
1 |
addCommand("getBeta", |
65 |
✓✗ | 1 |
makeDirectGetter(*this, &m_beta, |
66 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("Beta parameter", "double"))); |
67 |
✓✗✓✗ |
1 |
addCommand("setBeta", |
68 |
✓✗ | 1 |
makeCommandVoid1( |
69 |
*this, &MadgwickAHRS::set_beta, |
||
70 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the filter parameter beta", "double"))); |
71 |
✓✗✓✗ |
1 |
addCommand("set_imu_quat", |
72 |
✓✗ | 1 |
makeCommandVoid1( |
73 |
*this, &MadgwickAHRS::set_imu_quat, |
||
74 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the quaternion as [w,x,y,z]", "vector"))); |
75 |
1 |
} |
|
76 |
|||
77 |
1 |
void MadgwickAHRS::init(const double &dt) { |
|
78 |
✗✓✗✗ ✗✗✗✗ |
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 |
✓✗✗✓ |
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 |
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 |
✓✗✓✗ |
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 |
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 |
✓✗✓✗ |
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 |
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 |
✓✗✗✓ ✗✗✗✗ |
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 |
| Generated by: GCOVR (Version 4.2) |