Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.

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

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