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.
Mahony/MahonyAHRS.h
- Committer:
- Anaesthetix
- Date:
- 2018-07-31
- Revision:
- 8:981f7e2365b6
File content as of revision 8:981f7e2365b6:
//============================================================================================= // MahonyAHRS.h //============================================================================================= // // Madgwick's implementation of Mayhony's AHRS algorithm. // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // //============================================================================================= #ifndef MahonyAHRS_h #define MahonyAHRS_h #include <math.h> //-------------------------------------------------------------------------------------------- // Variable declaration class Mahony { private: float twoKp; // 2 * proportional gain (Kp) float twoKi; // 2 * integral gain (Ki) float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki // float invSampleFreq; float roll, pitch, yaw; char anglesComputed; static float invSqrt(float x); void computeAngles(); //------------------------------------------------------------------------------------------- // Function declarations public: Mahony(); float invSampleFreq; void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); float getRoll() { if (!anglesComputed) computeAngles(); return roll * 57.29578f; } float getPitch() { if (!anglesComputed) computeAngles(); return pitch * 57.29578f; } float getYaw() { if (!anglesComputed) computeAngles(); return yaw * 57.29578f + 180.0f; } float getRollRadians() { if (!anglesComputed) computeAngles(); return roll; } float getPitchRadians() { if (!anglesComputed) computeAngles(); return pitch; } float getYawRadians() { if (!anglesComputed) computeAngles(); return yaw; } }; #endif