File content as of revision 14:6cb3c2204b9b:

#ifndef __DCM_H
#define __DCM_H


#define DEG2RAD(x) (x * 0.01745329252)  // *pi/180
#define RAD2DEG(x) (x * 57.2957795131)  // *180/pi

// Default values 0.2 0.00002 1.2 0.00002
#define Kp_ROLLPITCH 0.1
#define Ki_ROLLPITCH 0.00001
#define Kp_YAW 0.1
#define Ki_YAW 0.00002


#define OUTPUTMODE 1 // enable drift correction

class DCM {
    public:
        /** Output: Euler angle: roll */
        float roll;
        /** Output:  Euler angle: pitch */
        float pitch;
        /** Output:  Euler angle: yaw */
        float yaw;
                       
        /** Input for the integration time (DCM algorithm) */
        float G_Dt;

        /** Creates a new DCM AHRS algorithm object
         */
        DCM(void);

        /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
         * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
         * than gyro/accelerometer-based update
         */
        void Update_step(float dt, float AccelV[], float GyroV[], float MagnV[]);
        void reset_sensor_fusion(float AccelV[], float GyroV[], float MagnV[]);

        float dcm[3][3];        // The Direction Cosine Matrix
    private:
        /** Input for the calculated magnetic heading */
        float Accel_Vector[3];
        float Gyro_Vector[3];
        float Magn_Vector[3];
        float MAG_Heading;
        float errorRollPitch[3]; 
        float errorYaw[3];
        float Omega_Vector[3];  // Corrected Gyro_Vector data
        float Omega_P[3];       // Omega Proportional correction
        float Omega_I[3];       // Omega Integrator
        float Omega[3];         // Omega
        float Temporary_Matrix[3][3];
        float Update_Matrix[3][3];
        int update_count;   // call Update_mag() every update_count calls to Update_step()
        float constrain(float x, float a, float b);
        
        void Matrix_update(void);
        void Normalize(void);
        void Drift_correction(void);
        void Accel_adjust(void);
        void Euler_angles(void);
        void Update_mag(void);
        void Compass_Heading(void);
        void pv(float a[3]);
        void pm(float a[3][3]);
        
};

#endif