File content as of revision 1:054bfc57f0f9:

#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