Revision 0:69340ac25ae9, committed 2021-12-02

Comitter:

ericleal

Date:

Thu Dec 02 15:25:28 2021 +0000

Commit message:

Madgwick AHRS library for Mbed OS6 modified to take inconsistent sample frequencies.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Madgwick.cpp	Thu Dec 02 15:25:28 2021 +0000
@@ -0,0 +1,118 @@
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+// 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
+//
+//=============================================================================================
+#include "mbed.h"
+#include "Madgwick.h"
+
+Madgwick::Madgwick(float gain)
+{
+    beta = gain;
+}
+
+void Madgwick::update_attitude(float gx, float gy, float gz, float ax, float ay, float az, float sampleFreq) 
+{
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+// Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+// Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+// Auxiliary variables to avoid repeated arithmetic
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _4q0 = 4.0f * q0;
+    _4q1 = 4.0f * q1;
+    _4q2 = 4.0f * q2;
+    _8q1 = 8.0f * q1;
+    _8q2 = 8.0f * q2;
+    q0q0 = q0 * q0;
+    q1q1 = q1 * q1;
+    q2q2 = q2 * q2;
+    q3q3 = q3 * q3;
+
+// Gradient decent algorithm corrective step
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+// Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+// Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * sampleFreq;
+  q1 += qDot2 * sampleFreq;
+  q2 += qDot3 * sampleFreq;
+  q3 += qDot4 * sampleFreq;
+
+// Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+
+//Euler transformation
+    roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) * 57.2958;              //convert radians to degrees and multiply by 12 for esc resolution 57.2958 * 12 = 687.5496
+    pitch = asinf(-2.0f * (q1*q3 - q0*q2)) * 57.2958;
+    yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) * 57.2958;
+}
+
+float Madgwick::getRoll() {
+    return roll;
+}
+
+float Madgwick::getPitch() {
+    return pitch;
+}
+
+float Madgwick::getYaw() {
+    return yaw;
+}
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//speed square inverse
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Madgwick::invSqrt(float x) {
+  float halfx = 0.5f * x;
+  float y = x;
+  long i = *(long*)&y;
+  i = 0x5f3759df - (i>>1);
+  y = *(float*)&i;
+  y = y * (1.5f - (halfx * y * y));
+  return y;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Madgwick.h	Thu Dec 02 15:25:28 2021 +0000
@@ -0,0 +1,32 @@
+#ifndef Madgwick_H
+#define Madgwick_H
+
+#include "mbed.h"
+
+class Madgwick {
+protected:
+public:
+
+    Madgwick(float gain);
+    void update_attitude(float gx, float gy, float gz, float ax, float ay, float az, float sampleFreq);
+    float getRoll();
+    float getPitch();
+    float getYaw();
+    
+    
+protected: 
+private:
+  float invSqrt(float x);
+  float roll, pitch, yaw;
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+  volatile float beta;                                // 2 * proportional gain (Kp)
+  volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
+
+    
+};
+ 
+ #endif
+ 
\ No newline at end of file