Erik van de Coevering
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.
Diff: Mahony/MahonyAHRS.h
- Revision:
- 8:981f7e2365b6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mahony/MahonyAHRS.h Tue Jul 31 20:36:57 2018 +0000
@@ -0,0 +1,67 @@
+//=============================================================================================
+// 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
\ No newline at end of file