Diff: AHRS.h

Revision:

9:a9b0f8540cc6

Parent:

8:928673148b55

--- a/AHRS.h	Wed Jan 20 02:42:22 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,117 +0,0 @@
-#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