semin ahn / Mbed OS zeta_stm_kinetic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

MPU9250/MadgwickAHRS.h@0:4ff8aeb3e4d1, 2021-04-02 (annotated)

Committer:
_seminahn
Date:
Fri Apr 02 05:24:49 2021 +0000
Revision:
0:4ff8aeb3e4d1
top_module

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 //=============================================================================================
_seminahn 0:4ff8aeb3e4d1 2 // MadgwickAHRS.h
_seminahn 0:4ff8aeb3e4d1 3 //=============================================================================================
_seminahn 0:4ff8aeb3e4d1 4 //
_seminahn 0:4ff8aeb3e4d1 5 // Implementation of Madgwick's IMU and AHRS algorithms.
_seminahn 0:4ff8aeb3e4d1 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
_seminahn 0:4ff8aeb3e4d1 7 //
_seminahn 0:4ff8aeb3e4d1 8 // From the x-io website "Open-source resources available on this website are
_seminahn 0:4ff8aeb3e4d1 9 // provided under the GNU General Public Licence unless an alternative licence
_seminahn 0:4ff8aeb3e4d1 10 // is provided in source."
_seminahn 0:4ff8aeb3e4d1 11 //
_seminahn 0:4ff8aeb3e4d1 12 // Date Author Notes
_seminahn 0:4ff8aeb3e4d1 13 // 29/09/2011 SOH Madgwick Initial release
_seminahn 0:4ff8aeb3e4d1 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
_seminahn 0:4ff8aeb3e4d1 15 //
_seminahn 0:4ff8aeb3e4d1 16 //=============================================================================================
_seminahn 0:4ff8aeb3e4d1 17 #ifndef MadgwickAHRS_h
_seminahn 0:4ff8aeb3e4d1 18 #define MadgwickAHRS_h
_seminahn 0:4ff8aeb3e4d1 19 #include <math.h>
_seminahn 0:4ff8aeb3e4d1 20
_seminahn 0:4ff8aeb3e4d1 21 //--------------------------------------------------------------------------------------------
_seminahn 0:4ff8aeb3e4d1 22 // Variable declaration
_seminahn 0:4ff8aeb3e4d1 23 class Madgwick{
_seminahn 0:4ff8aeb3e4d1 24 private:
_seminahn 0:4ff8aeb3e4d1 25 static float invSqrt(float x);
_seminahn 0:4ff8aeb3e4d1 26 float beta; // algorithm gain
_seminahn 0:4ff8aeb3e4d1 27 float roll;
_seminahn 0:4ff8aeb3e4d1 28 float pitch;
_seminahn 0:4ff8aeb3e4d1 29 float yaw;
_seminahn 0:4ff8aeb3e4d1 30 char anglesComputed;
_seminahn 0:4ff8aeb3e4d1 31 void computeAngles();
_seminahn 0:4ff8aeb3e4d1 32
_seminahn 0:4ff8aeb3e4d1 33 //-------------------------------------------------------------------------------------------
_seminahn 0:4ff8aeb3e4d1 34 // Function declarations
_seminahn 0:4ff8aeb3e4d1 35 public:
_seminahn 0:4ff8aeb3e4d1 36 float invSampleFreq;
_seminahn 0:4ff8aeb3e4d1 37
_seminahn 0:4ff8aeb3e4d1 38 float q0;
_seminahn 0:4ff8aeb3e4d1 39 float q1;
_seminahn 0:4ff8aeb3e4d1 40 float q2;
_seminahn 0:4ff8aeb3e4d1 41 float q3; // quaternion of sensor frame relative to auxiliary frame
_seminahn 0:4ff8aeb3e4d1 42
_seminahn 0:4ff8aeb3e4d1 43 Madgwick(void);
_seminahn 0:4ff8aeb3e4d1 44 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
_seminahn 0:4ff8aeb3e4d1 45 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
_seminahn 0:4ff8aeb3e4d1 46 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
_seminahn 0:4ff8aeb3e4d1 47 //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
_seminahn 0:4ff8aeb3e4d1 48 //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
_seminahn 0:4ff8aeb3e4d1 49 //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
_seminahn 0:4ff8aeb3e4d1 50 float getRoll() {
_seminahn 0:4ff8aeb3e4d1 51 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 52 return roll * 57.29578f;
_seminahn 0:4ff8aeb3e4d1 53 }
_seminahn 0:4ff8aeb3e4d1 54 float getPitch() {
_seminahn 0:4ff8aeb3e4d1 55 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 56 return pitch * 57.29578f;
_seminahn 0:4ff8aeb3e4d1 57 }
_seminahn 0:4ff8aeb3e4d1 58 float getYaw() {
_seminahn 0:4ff8aeb3e4d1 59 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 60 return yaw * 57.29578f + 180.0f;
_seminahn 0:4ff8aeb3e4d1 61 }
_seminahn 0:4ff8aeb3e4d1 62 float getRollRadians() {
_seminahn 0:4ff8aeb3e4d1 63 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 64 return roll;
_seminahn 0:4ff8aeb3e4d1 65 }
_seminahn 0:4ff8aeb3e4d1 66 float getPitchRadians() {
_seminahn 0:4ff8aeb3e4d1 67 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 68 return pitch;
_seminahn 0:4ff8aeb3e4d1 69 }
_seminahn 0:4ff8aeb3e4d1 70 float getYawRadians() {
_seminahn 0:4ff8aeb3e4d1 71 if (!anglesComputed) computeAngles();
_seminahn 0:4ff8aeb3e4d1 72 return yaw;
_seminahn 0:4ff8aeb3e4d1 73 }
_seminahn 0:4ff8aeb3e4d1 74 };
_seminahn 0:4ff8aeb3e4d1 75 #endif
_seminahn 0:4ff8aeb3e4d1 76
_seminahn 0:4ff8aeb3e4d1 77