Diff: AHRS.h

Revision:

0:bf9febe45e1d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.h	Wed Jan 20 14:50:58 2016 +0000
@@ -0,0 +1,119 @@
+#ifndef _AHRS_
+#define _AHRS_
+
+#include "MPU9250.h"
+
+// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
+// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
+// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
+// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
+// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
+// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+
+class AHRS:public MPU9250
+{
+private:
+    float roll, pitch, yaw;
+    float q[4];           // vector to hold quaternion
+    float eInt[3];              // vector to hold integral error for Mahony method
+
+    // parameters for 6 DoF sensor fusion calculations
+    float PI;
+    float GyroMeasError;     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+    float GyroMeasDrift;      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+    float beta;  // compute beta
+    float zeta;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+
+    float norm;
+    float hx, hy, bx, bz;
+    float vx, vy, vz, wx, wy, wz;
+    float ex, ey, ez;
+    float pa, pb, pc;
+
+    float q1, q2, q3, q4;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    float q1q1;
+    float q1q2;
+    float q1q3;
+    float q1q4;
+    float q2q2;
+    float q2q3;
+    float q2q4;
+    float q3q3;
+    float q3q4;
+    float q4q4;
+
+    float Xh;
+    float Yh;
+    float yawmag;
+
+    float deltat;
+    float delt_t;
+    float Now;
+    float sum;
+    float firstUpdate;
+    float lastUpdate;
+    uint32_t sumCount;
+    int count;
+    //int16_t tempCount;
+
+
+public:
+    //MPU9250 imu;
+    //Serial test;
+    Timer t;
+    Serial pc2;
+    AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address) : MPU9250(sda,scl,tx,rx,address),pc2(tx,rx) {
+        for(int i=0; i<=3; i++) {
+            eInt[i] = 0;
+            q[i] = 0;     // vector to hold quaternion
+        }
+
+        q[0] = 1.0f;
+
+        PI = 3.14159265358979323846f;
+        GyroMeasError = PI * (60.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+        beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
+        GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+        zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+
+        //Time
+        sum = 0;
+        sumCount = 0;
+        count = 0;
+        
+        delt_t = 0;
+        lastUpdate = 0;
+        firstUpdate = 0;
+        Now = 0;
+    };
+    //void MadgwickQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
+    //void MahonyQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
+
+    void MadgwickQuaternionUpdate();
+    void MahonyQuaternionUpdate();
+
+    float getRoll() {
+        return roll;
+    }
+    float getPitch() {
+        return pitch;
+    }
+    float getYaw() {
+        return yaw;
+    }
+    void PrintRollPitchYaw();
+
+    void TimeStart();
+    
+    void TimeRead() {
+        t.read_us();
+    };
+    
+    void TimeCal();
+    
+    void Run();
+
+};
+#endif
\ No newline at end of file