Latest version of my quadcopter controller with an LPC1768 and MPU9250.
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?
User | Revision | Line number | New 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 |