Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

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:   mbed

Mahony/MahonyAHRS.h@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 8:981f7e2365b6 1 //=============================================================================================
Anaesthetix 8:981f7e2365b6 2 // MahonyAHRS.h
Anaesthetix 8:981f7e2365b6 3 //=============================================================================================
Anaesthetix 8:981f7e2365b6 4 //
Anaesthetix 8:981f7e2365b6 5 // Madgwick's implementation of Mayhony's AHRS algorithm.
Anaesthetix 8:981f7e2365b6 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Anaesthetix 8:981f7e2365b6 7 //
Anaesthetix 8:981f7e2365b6 8 // Date Author Notes
Anaesthetix 8:981f7e2365b6 9 // 29/09/2011 SOH Madgwick Initial release
Anaesthetix 8:981f7e2365b6 10 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
Anaesthetix 8:981f7e2365b6 11 //
Anaesthetix 8:981f7e2365b6 12 //=============================================================================================
Anaesthetix 8:981f7e2365b6 13 #ifndef MahonyAHRS_h
Anaesthetix 8:981f7e2365b6 14 #define MahonyAHRS_h
Anaesthetix 8:981f7e2365b6 15 #include <math.h>
Anaesthetix 8:981f7e2365b6 16
Anaesthetix 8:981f7e2365b6 17 //--------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 18 // Variable declaration
Anaesthetix 8:981f7e2365b6 19
Anaesthetix 8:981f7e2365b6 20 class Mahony {
Anaesthetix 8:981f7e2365b6 21 private:
Anaesthetix 8:981f7e2365b6 22 float twoKp; // 2 * proportional gain (Kp)
Anaesthetix 8:981f7e2365b6 23 float twoKi; // 2 * integral gain (Ki)
Anaesthetix 8:981f7e2365b6 24 float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
Anaesthetix 8:981f7e2365b6 25 float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
Anaesthetix 8:981f7e2365b6 26 // float invSampleFreq;
Anaesthetix 8:981f7e2365b6 27 float roll, pitch, yaw;
Anaesthetix 8:981f7e2365b6 28 char anglesComputed;
Anaesthetix 8:981f7e2365b6 29 static float invSqrt(float x);
Anaesthetix 8:981f7e2365b6 30 void computeAngles();
Anaesthetix 8:981f7e2365b6 31
Anaesthetix 8:981f7e2365b6 32 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 33 // Function declarations
Anaesthetix 8:981f7e2365b6 34
Anaesthetix 8:981f7e2365b6 35 public:
Anaesthetix 8:981f7e2365b6 36 Mahony();
Anaesthetix 8:981f7e2365b6 37 float invSampleFreq;
Anaesthetix 8:981f7e2365b6 38 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
Anaesthetix 8:981f7e2365b6 39 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
Anaesthetix 8:981f7e2365b6 40 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
Anaesthetix 8:981f7e2365b6 41 float getRoll() {
Anaesthetix 8:981f7e2365b6 42 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 43 return roll * 57.29578f;
Anaesthetix 8:981f7e2365b6 44 }
Anaesthetix 8:981f7e2365b6 45 float getPitch() {
Anaesthetix 8:981f7e2365b6 46 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 47 return pitch * 57.29578f;
Anaesthetix 8:981f7e2365b6 48 }
Anaesthetix 8:981f7e2365b6 49 float getYaw() {
Anaesthetix 8:981f7e2365b6 50 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 51 return yaw * 57.29578f + 180.0f;
Anaesthetix 8:981f7e2365b6 52 }
Anaesthetix 8:981f7e2365b6 53 float getRollRadians() {
Anaesthetix 8:981f7e2365b6 54 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 55 return roll;
Anaesthetix 8:981f7e2365b6 56 }
Anaesthetix 8:981f7e2365b6 57 float getPitchRadians() {
Anaesthetix 8:981f7e2365b6 58 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 59 return pitch;
Anaesthetix 8:981f7e2365b6 60 }
Anaesthetix 8:981f7e2365b6 61 float getYawRadians() {
Anaesthetix 8:981f7e2365b6 62 if (!anglesComputed) computeAngles();
Anaesthetix 8:981f7e2365b6 63 return yaw;
Anaesthetix 8:981f7e2365b6 64 }
Anaesthetix 8:981f7e2365b6 65 };
Anaesthetix 8:981f7e2365b6 66
Anaesthetix 8:981f7e2365b6 67 #endif