Master Thesis  V1.0
Research and Design of Sensor Node for NMSD Treatment
Macros | Functions | Variables
MadgwickAHRS.c File Reference

Sensor fusion. More...

#include "MadgwickAHRS.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
Include dependency graph for MadgwickAHRS.c:

Go to the source code of this file.

Macros

#define sampleFreq   51.136f
 
#define M_PI   3.14159265358979323846
 

Functions

float invSqrt (float x)
 
void MadgwickAHRSupdate (float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
 Madgwick algorithm. More...
 
void MadgwickAHRSupdateIMU (float gx, float gy, float gz, float ax, float ay, float az)
 Madgwick algorithm. More...
 
void QuaternionsToEulerAngles (float *euler_angles)
 Convert quaternions to euler angles. More...
 

Variables

volatile float q0 = 1.0f
 
volatile float q1 = 0.0f
 
volatile float q2 = 0.0f
 
volatile float q3 = 0.0f
 
volatile float yaw =0.0f
 
volatile float pitch =0.0f
 
volatile float roll =0.0f
 

Detailed Description

Sensor fusion.

Version
1.0
Author
Jona Cappelle

Definition in file MadgwickAHRS.c.

Macro Definition Documentation

◆ M_PI

#define M_PI   3.14159265358979323846

Definition of PI

Definition at line 39 of file MadgwickAHRS.c.

◆ sampleFreq

#define sampleFreq   51.136f

sample frequency in Hz

Definition at line 35 of file MadgwickAHRS.c.

Function Documentation

◆ invSqrt()

float invSqrt ( float  x)

Calculate inverse sqrt

Definition at line 289 of file MadgwickAHRS.c.

289  {
290  float halfx = 0.5f * x;
291  float y = x;
292  long i = *(long*)&y;
293  i = 0x5f3759df - (i>>1);
294  y = *(float*)&i;
295  y = y * (1.5f - (halfx * y * y));
296  return y;
297 }

Referenced by MadgwickAHRSupdate(), and MadgwickAHRSupdateIMU().

◆ MadgwickAHRSupdate()

void MadgwickAHRSupdate ( float  gx,
float  gy,
float  gz,
float  ax,
float  ay,
float  az,
float  mx,
float  my,
float  mz 
)

Madgwick algorithm.

9 DoF sensor fusion

Note
Gyro + Accel + Magn fusion
Parameters
[in]gxGyro x
[in]gyGyro y
[in]gzGyro a
[in]axAccel x
[in]ayAccel y
[in]azAccel a
[in]mxMagn x
[in]myMagn y
[in]mzMagn a

Definition at line 92 of file MadgwickAHRS.c.

92  {
93  float recipNorm;
94  float s0, s1, s2, s3;
95  float qDot1, qDot2, qDot3, qDot4;
96  float hx, hy;
97  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
98 
99  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
100  if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
101  MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
102  return;
103  }
104 
105  // Rate of change of quaternion from gyroscope
106  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
107  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
108  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
109  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
110 
111  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
112  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
113 
114  // Normalise accelerometer measurement
115  recipNorm = invSqrt(ax * ax + ay * ay + az * az);
116  ax *= recipNorm;
117  ay *= recipNorm;
118  az *= recipNorm;
119 
120  // Normalise magnetometer measurement
121  recipNorm = invSqrt(mx * mx + my * my + mz * mz);
122  mx *= recipNorm;
123  my *= recipNorm;
124  mz *= recipNorm;
125 
126  // Auxiliary variables to avoid repeated arithmetic
127  _2q0mx = 2.0f * q0 * mx;
128  _2q0my = 2.0f * q0 * my;
129  _2q0mz = 2.0f * q0 * mz;
130  _2q1mx = 2.0f * q1 * mx;
131  _2q0 = 2.0f * q0;
132  _2q1 = 2.0f * q1;
133  _2q2 = 2.0f * q2;
134  _2q3 = 2.0f * q3;
135  _2q0q2 = 2.0f * q0 * q2;
136  _2q2q3 = 2.0f * q2 * q3;
137  q0q0 = q0 * q0;
138  q0q1 = q0 * q1;
139  q0q2 = q0 * q2;
140  q0q3 = q0 * q3;
141  q1q1 = q1 * q1;
142  q1q2 = q1 * q2;
143  q1q3 = q1 * q3;
144  q2q2 = q2 * q2;
145  q2q3 = q2 * q3;
146  q3q3 = q3 * q3;
147 
148  // Reference direction of Earth's magnetic field
149  hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
150  hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
151  _2bx = sqrt(hx * hx + hy * hy);
152  _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
153  _4bx = 2.0f * _2bx;
154  _4bz = 2.0f * _2bz;
155 
156  // Gradient decent algorithm corrective step
157  s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
158  s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
159  s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
160  s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
161  recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
162  s0 *= recipNorm;
163  s1 *= recipNorm;
164  s2 *= recipNorm;
165  s3 *= recipNorm;
166 
167  // Apply feedback step
168  qDot1 -= beta * s0;
169  qDot2 -= beta * s1;
170  qDot3 -= beta * s2;
171  qDot4 -= beta * s3;
172  }
173 
174  // Integrate rate of change of quaternion to yield quaternion
175  q0 += qDot1 * (1.0f / sampleFreq);
176  q1 += qDot2 * (1.0f / sampleFreq);
177  q2 += qDot3 * (1.0f / sampleFreq);
178  q3 += qDot4 * (1.0f / sampleFreq);
179 
180  // Normalise quaternion
181  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
182  q0 *= recipNorm;
183  q1 *= recipNorm;
184  q2 *= recipNorm;
185  q3 *= recipNorm;
186 }

References beta, invSqrt(), MadgwickAHRSupdateIMU(), q0, q1, q2, q3, and sampleFreq.

Referenced by measure_send().

◆ MadgwickAHRSupdateIMU()

void MadgwickAHRSupdateIMU ( float  gx,
float  gy,
float  gz,
float  ax,
float  ay,
float  az 
)

Madgwick algorithm.

6 DoF sensor fusion

Note
Gyro + Accel fusion
Parameters
[in]gxGyro x
[in]gyGyro y
[in]gzGyro a
[in]axAccel x
[in]ayAccel y
[in]azAccel a

Definition at line 217 of file MadgwickAHRS.c.

217  {
218  float recipNorm;
219  float s0, s1, s2, s3;
220  float qDot1, qDot2, qDot3, qDot4;
221  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
222 
223  // Rate of change of quaternion from gyroscope
224  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
225  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
226  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
227  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
228 
229  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
230  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
231 
232  // Normalise accelerometer measurement
233  recipNorm = invSqrt(ax * ax + ay * ay + az * az);
234  ax *= recipNorm;
235  ay *= recipNorm;
236  az *= recipNorm;
237 
238  // Auxiliary variables to avoid repeated arithmetic
239  _2q0 = 2.0f * q0;
240  _2q1 = 2.0f * q1;
241  _2q2 = 2.0f * q2;
242  _2q3 = 2.0f * q3;
243  _4q0 = 4.0f * q0;
244  _4q1 = 4.0f * q1;
245  _4q2 = 4.0f * q2;
246  _8q1 = 8.0f * q1;
247  _8q2 = 8.0f * q2;
248  q0q0 = q0 * q0;
249  q1q1 = q1 * q1;
250  q2q2 = q2 * q2;
251  q3q3 = q3 * q3;
252 
253  // Gradient decent algorithm corrective step
254  s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
255  s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
256  s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
257  s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
258  recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
259  s0 *= recipNorm;
260  s1 *= recipNorm;
261  s2 *= recipNorm;
262  s3 *= recipNorm;
263 
264  // Apply feedback step
265  qDot1 -= beta * s0;
266  qDot2 -= beta * s1;
267  qDot3 -= beta * s2;
268  qDot4 -= beta * s3;
269  }
270 
271  // Integrate rate of change of quaternion to yield quaternion
272  q0 += qDot1 * (1.0f / sampleFreq);
273  q1 += qDot2 * (1.0f / sampleFreq);
274  q2 += qDot3 * (1.0f / sampleFreq);
275  q3 += qDot4 * (1.0f / sampleFreq);
276 
277  // Normalise quaternion
278  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
279  q0 *= recipNorm;
280  q1 *= recipNorm;
281  q2 *= recipNorm;
282  q3 *= recipNorm;
283 }

References beta, invSqrt(), q0, q1, q2, q3, and sampleFreq.

Referenced by MadgwickAHRSupdate().

◆ QuaternionsToEulerAngles()

void QuaternionsToEulerAngles ( float *  euler_angles)

Convert quaternions to euler angles.

Note
Euler angles are less accurate and suffer from Gimbal Lock
Parameters
[out]euler_anglespointer to location of euler angles storage

Definition at line 351 of file MadgwickAHRS.c.

352 {
353 
354  // roll (x-axis rotation)
355  double sinr_cosp = 2 * (q0 * q1 + q2 * q3);
356  double cosr_cosp = 1 - 2 * (q1 * q1 + q2 * q2);
357  roll = atan2(sinr_cosp, cosr_cosp);
358 
359  // pitch (y-axis rotation)
360  double sinp = 2 * (q0 * q2 - q3 * q1);
361  if (abs(sinp) >= 1)
362  pitch = copysign(M_PI/ 2, sinp); // use 90 degrees if out of range
363  else
364  pitch = asin(sinp);
365 
366  // yaw (z-axis rotation)
367  double siny_cosp = 2 * (q0 * q3 + q1 * q2);
368  double cosy_cosp = 1 - 2 * (q2 * q2 + q3 * q3);
369  yaw = atan2(siny_cosp, cosy_cosp);
370 
371 /* Pass pointers through to main file */
372  euler_angles[0] = roll;
373  euler_angles[1] = pitch;
374  euler_angles[2] = yaw;
375 
376 }

References M_PI, pitch, q0, q1, q2, q3, roll, and yaw.

Referenced by measure_send().

Variable Documentation

◆ pitch

volatile float pitch =0.0f

Definition at line 46 of file MadgwickAHRS.c.

Referenced by QuaternionsToEulerAngles().

◆ q0

volatile float q0 = 1.0f

Definition at line 45 of file MadgwickAHRS.c.

Referenced by MadgwickAHRSupdate(), MadgwickAHRSupdateIMU(), and QuaternionsToEulerAngles().

◆ q1

volatile float q1 = 0.0f

Definition at line 45 of file MadgwickAHRS.c.

Referenced by MadgwickAHRSupdate(), MadgwickAHRSupdateIMU(), and QuaternionsToEulerAngles().

◆ q2

volatile float q2 = 0.0f

Definition at line 45 of file MadgwickAHRS.c.

Referenced by MadgwickAHRSupdate(), MadgwickAHRSupdateIMU(), and QuaternionsToEulerAngles().

◆ q3

volatile float q3 = 0.0f

quaternion of sensor frame relative to auxiliary frame

Definition at line 45 of file MadgwickAHRS.c.

Referenced by MadgwickAHRSupdate(), MadgwickAHRSupdateIMU(), and QuaternionsToEulerAngles().

◆ roll

volatile float roll =0.0f

Yaw, pith, roll result

Definition at line 46 of file MadgwickAHRS.c.

Referenced by QuaternionsToEulerAngles().

◆ yaw

volatile float yaw =0.0f

Definition at line 46 of file MadgwickAHRS.c.

Referenced by QuaternionsToEulerAngles().

yaw
volatile float yaw
Definition: MadgwickAHRS.c:46
beta
volatile float beta
Definition: main.c:145
pitch
volatile float pitch
Definition: MadgwickAHRS.c:46
MadgwickAHRSupdateIMU
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
Madgwick algorithm.
Definition: MadgwickAHRS.c:217
q1
volatile float q1
Definition: MadgwickAHRS.c:45
sampleFreq
#define sampleFreq
Definition: MadgwickAHRS.c:35
q2
volatile float q2
Definition: MadgwickAHRS.c:45
M_PI
#define M_PI
Definition: MadgwickAHRS.c:39
q3
volatile float q3
Definition: MadgwickAHRS.c:45
invSqrt
float invSqrt(float x)
Definition: MadgwickAHRS.c:289
q0
volatile float q0
Definition: MadgwickAHRS.c:45
roll
volatile float roll
Definition: MadgwickAHRS.c:46