File content as of revision 0:62a1c91a859a:

// ===============================================================================================
// =                              UAVXArm Quadrocopter Controller                                =
// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
// ===============================================================================================

//    This is part of UAVXArm.

//    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
//    General Public License as published by the Free Software Foundation, either version 3 of the
//    License, or (at your option) any later version.

//    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
//    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//    See the GNU General Public License for more details.

//    You should have received a copy of the GNU General Public License along with this program.
//    If not, see http://www.gnu.org/licenses/

#include "UAVXArm.h"

//#define USE_GYRO_RATE

// Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.

void AttitudeFailsafeEstimate(void);
void DoLegacyYawComp(void);
void AttitudeTest(void);

real32 dT, HalfdT, dTR, dTmS;
uint32 uSp;
uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS;

void AttitudeFailsafeEstimate(void) {

    static uint8 i;

    for ( i = 0; i < (uint8)3; i++ ) {
        Rate[i] = Gyro[i];
        Angle[i] += Rate[i] * dTmS;
    }
} // AttitudeFailsafeEstimate

void DoLegacyYawComp(void) {

    static real32 Temp, HE;

    if ( F.CompassValid ) {
        // + CCW
        Temp = DesiredYaw - Trim[Yaw];
        if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading
            DesiredHeading = Heading;
            HE = 0.0;
        } else {
            HE = MakePi(DesiredHeading - Heading);
            HE = Limit(HE, -SIXTHPI, SIXTHPI); // 30 deg limit
            HE = HE * K[CompassKp];
            Rate[Yaw] -= Limit(HE, -COMPASS_MAXDEV, COMPASS_MAXDEV);
        }
    }

    Angle[Yaw] += Rate[Yaw];
    Angle[Yaw] = Limit(Angle[Yaw], -K[YawIntLimit], K[YawIntLimit]);

    Angle[Yaw] = DecayX(Angle[Yaw], 0.0002);                 // GKE added to kill gyro drift

} // DoLegacyYawComp

void GetAttitude(void) {

    static uint32 Now;
    static uint8 i;

    if ( GyroType == IRSensors )
        GetIRAttitude();
    else {
        GetGyroRates();
        GetAccelerations();
    }

    if ( mSClock() > mS[CompassUpdate] ) {
        mS[CompassUpdate] = mSClock() + COMPASS_UPDATE_MS;
        GetHeading();
#ifndef USE_DCM_YAW
        DoLegacyYawComp();
#endif // !USE_DCM_YAW
    }

    Now = uSClock();
    dT = ( Now - uSp) * 0.000001;
    HalfdT = 0.5 * dT;
    dTR = 1.0 / dT;
    uSp = Now;

    if ( GyroType == IRSensors ) {

        for ( i = 0; i < (uint8)2; i++ ) {
            Rate[i] = ( Angle[i] - Anglep[i] ) * dT;
            Anglep[i] = Angle[i];
        }

        Rate[Yaw] = 0.0; // need Yaw gyro!

    } else {
        DebugPin = true;
        switch  ( AttitudeMethod ) {
            case PremerlaniDCM:
                DCMUpdate();
                DCMNormalise();
                DCMDriftCorrection();
                DCMEulerAngles();
                break;
            case MadgwickIMU:
                IMUupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD]);
                EulerAngles();
                //   DoLegacyYawComp();
                break;
            case MadgwickAHRS: // must have magnetometer
                AHRSupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD], 1,0,0);//Mag[BF].V, Mag[LR].V, Mag[UD].V);
                EulerAngles();
                break;
        } // switch

        DebugPin = false;
    }

    F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT;

} // GetAttitude


void AttitudeTest(void) {

    TxString("\r\nAttitude Test\r\n");

    GetAttitude();

    TxString("\r\ndT \t");
    TxVal32(dT * 1000.0, 3, 0);
    TxString(" Sec.\r\n\r\n");

    if ( GyroType == IRSensors ) {

        TxString("IR Sensors:\r\n");
        TxString("\tRoll \t");
        TxVal32(IR[Roll] * 100.0, 2, HT);
        TxNextLine();
        TxString("\tPitch\t");
        TxVal32(IR[Pitch] * 100.0, 2, HT);
        TxNextLine();
        TxString("\tZ    \t");
        TxVal32(IR[Yaw] * 100.0, 2, HT);
        TxNextLine();
        TxString("\tMin/Max\t");
        TxVal32(IRMin * 100.0, 2, HT);
        TxVal32(IRMax * 100.0, 2, HT);
        TxString("\tSwing\t");
        TxVal32(IRSwing * 100.0, 2, HT);
        TxNextLine();

    } else {

        TxString("Rates (Raw and Compensated):\r\n");
        TxString("\tRoll \t");
        TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
        TxVal32(Rate[Roll] * MILLIANGLE, 3, 0);
        TxNextLine();
        TxString("\tPitch\t");
        TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
        TxVal32(Rate[Pitch] * MILLIANGLE, 3, 0);
        TxNextLine();
        TxString("\tYaw  \t");
        TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
        TxVal32(Rate[Yaw] * MILLIANGLE, 3, 0);
        TxNextLine();

        TxString("Accelerations:\r\n");
        TxString("\tB->F \t");
        TxVal32(Acc[BF] * 1000.0, 3, 0);
        TxNextLine();
        TxString("\tL->R \t");
        TxVal32(Acc[LR] * 1000.0, 3, 0);
        TxNextLine();
        TxString("\tU->D \t");
        TxVal32(Acc[UD] * 1000.0, 3, 0);
        TxNextLine();
    }

    if ( CompassType != HMC6352 ) {
        TxString("Magnetic:\r\n");
        TxString("\tX    \t");
        TxVal32(Mag[Roll].V, 0, 0);
        TxNextLine();
        TxString("\tY    \t");
        TxVal32(Mag[Pitch].V, 0, 0);
        TxNextLine();
        TxString("\tZ    \t");
        TxVal32(Mag[Yaw].V, 0, 0);
        TxNextLine();
    }

    TxString("Compass: \t");
    TxVal32(Make2Pi(MagHeading) * MILLIANGLE, 3, 0);
    TxNextLine();

} // AttitudeTest

//____________________________________________________________________________________________

// The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled:
// Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
// work by R. Mahony et al. - Thanks Rob!

void DCMNormalise(void);
void DCMDriftCorrection(void);
void DCMMotionCompensation(void);
void DCMUpdate(void);
void DCMEulerAngles(void);

// requires rescaling for the much faster PID cycle in UAVXArm
// guess x5 initially
#define Kp_RollPitch 25.0       // 5.0
#define Ki_RollPitch 0.025      // 0.005
#define Kp_Yaw 1.2
#define Ki_Yaw 0.00002

real32   RollPitchError[3] = {0,0,0};
real32   OmegaV[3] = {0,0,0}; // corrected gyro data
real32   OmegaP[3] = {0,0,0}; // proportional correction
real32   OmegaI[3] = {0,0,0}; // integral correction
real32   Omega[3] = {0,0,0};
real32   DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}};
real32   U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}};
real32   TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}};

void DCMNormalise(void) {

    static real32 Error = 0;
    static real32 Renorm = 0.0;
    static boolean Problem;
    static uint8 r;

    Error = -VDot(&DCM[0][0], &DCM[1][0]) * 0.5;        //eq.19

    VScale(&TempM[0][0], &DCM[1][0], Error);            //eq.19
    VScale(&TempM[1][0], &DCM[0][0], Error);            //eq.19

    VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]);       //eq.19
    VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]);       //eq.19

    VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]);    // c= a * b eq.20

    Problem = false;
    for ( r = 0; r < (uint8)3; r++ ) {
        Renorm = VDot(&TempM[r][0],&TempM[r][0]);
        if ( (Renorm <  1.5625) && (Renorm > 0.64) )
            Renorm = 0.5 * (3.0 - Renorm);               //eq.21
        else
            if ( (Renorm < 100.0) && (Renorm > 0.01) )
                Renorm = 1.0 / sqrt( Renorm );
            else
                Problem = true;

        VScale(&DCM[r][0], &TempM[r][0], Renorm);
    }

    if ( Problem ) { // Divergent - force to initial conditions and hope!
        DCM[0][0] = 1.0;
        DCM[0][1] = 0.0;
        DCM[0][2] = 0.0;
        DCM[1][0] = 0.0;
        DCM[1][1] = 1.0;
        DCM[1][2] = 0.0;
        DCM[2][0] = 0.0;
        DCM[2][1] = 0.0;
        DCM[2][2] = 1.0;
    }

} // DCMNormalise

void DCMMotionCompensation(void) {
    // compensation for rate of change of velocity LR/BF needs GPS velocity but
    // updates probably too slow?
    Acc[LR ] += 0.0;
    Acc[BF] += 0.0;
} // DCMMotionCompensation

void DCMDriftCorrection(void) {

    static real32 ScaledOmegaP[3], ScaledOmegaI[3];
    static real32 YawError[3];
    static real32 AccMagnitude, AccWeight;
    static real32 ErrorCourse;

    AccMagnitude = sqrt( Sqr(Acc[0]) + Sqr(Acc[1]) + Sqr(Acc[2]) );

    // dynamic weighting of Accelerometer info (reliability filter)
    // weight for Accelerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0)
    AccWeight = Limit(1.0 - 2.0 * fabs(1.0 - AccMagnitude), 0.0, 1.0);

    VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground
    VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight);

    VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
    VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);

#ifdef USE_DCM_YAW
    // Yaw - drift correction based on compass/magnetometer heading
    HeadingCos = cos( Heading );
    HeadingSin = sin( Heading );
    ErrorCourse = ( U[0][0] * HeadingSin ) - ( U[1][0] * HeadingCos );
    VScale(YawError, &U[2][0], ErrorCourse );

    VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw );
    VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]);

    VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
    VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
#endif // USE_DCM_YAW
} // DCMDriftCorrection

void DCMUpdate(void) {

    static uint8 i, j, k;
    static real32 op[3];

    VAdd(&Omega[0], &Gyro[0], &OmegaI[0]);
    VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]);

//   MotionCompensation();

    U[0][0] =  0.0;
    U[0][1] = -dT * OmegaV[2];   //-z
    U[0][2] =  dT * OmegaV[1];   // y
    U[1][0] =  dT * OmegaV[2];   // z
    U[1][1] =  0.0;
    U[1][2] = -dT * OmegaV[0];   //-x
    U[2][0] = -dT * OmegaV[1];   //-y
    U[2][1] =  dT * OmegaV[0];   // x
    U[2][2] =  0.0;

    for ( i = 0; i < (uint8)3; i++ )
        for ( j = 0; j < (uint8)3; j++ ) {
            for ( k = 0; k < (uint8)3; k++ )
                op[k] = DCM[i][k] * U[k][j];

            TempM[i][j] = op[0] + op[1] + op[2];
        }

    for ( i = 0; i < (uint8)3; i++ )
        for (j = 0; j < (uint8)3; j++ )
            DCM[i][j] += TempM[i][j];

} // DCMUpdate

void DCMEulerAngles(void) {

    static uint8 g;

    for ( g = 0; g < (uint8)3; g++ )
        Rate[g] = Gyro[g];

    Angle[Pitch] = asin(DCM[2][0]);
    Angle[Roll] = -atan2(DCM[2][1], DCM[2][2]);

#ifdef USE_DCM_YAW
    Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]);
#endif // USE_DCM_YAW

} // DCMEulerAngles

//___________________________________________________________________________________

// IMU.c
// S.O.H. Madgwick
// 25th September 2010

// Description:

// Quaternion implementation of the 'DCM filter' [Mahony et al.].

// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.

// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.

// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.

#include <math.h>

void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az);

#define MKp 2.0          // proportional gain governs rate of convergence to accelerometer/magnetometer
#define MKi 0.005f        // integral gain governs rate of convergence of gyroscope biases

real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;  // quaternion elements representing the estimated orientation
real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0;   // scaled integral error

void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {

    static real32 rnorm;
    static real32 vx, vy, vz;
    static real32 ex, ey, ez;
    static real32 aMag;

//swap z and y?

    // normalise the measurements
    aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero Acc values into AHRS is FATAL
    if ( aMag < 0.9 ) {
        ax = ay = 0.0;
        az = 1.0;
    } else {
        rnorm = 1.0/aMag;
        ax *= rnorm;
        ay *= rnorm;
        az *= rnorm;
    }

    // estimated direction of gravity
    vx = 2.0*(q1*q3 - q0*q2);
    vy = 2.0*(q0*q1 + q2*q3);
    vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);

    // error is sum of cross product between reference direction of field and direction measured by sensor
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    // integral error scaled integral gain
    exInt += ex*MKi;
    eyInt += ey*MKi;
    ezInt += ez*MKi;

    // adjusted gyroscope measurements
    gx += MKp*ex + exInt;
    gy += MKp*ey + eyInt;
    gz += MKp*ez + ezInt;

    // integrate quaternion rate and normalise
    q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
    q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
    q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
    q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;

    // normalise quaternion
    rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
    q0 *= rnorm;
    q1 *= rnorm;
    q2 *= rnorm;
    q3 *= rnorm;

}  // IMUupdate

//_________________________________________________________________________________

// AHRS.c
// S.O.H. Madgwick
// 25th August 2010

// Description:

// Quaternion implementation of the 'DCM filter' [Mahoney et al].  Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.

// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.

// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.

// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.

void AHRSupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {

    static real32 rnorm;
    static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2;
    static real32 vx, vy, vz, wx, wy, wz;
    static real32 ex, ey, ez;
    static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
    static real32 aMag;

    // auxiliary variables to reduce number of repeated operations
    q0q0 = q0*q0;
    q0q1 = q0*q1;
    q0q2 = q0*q2;
    q0q3 = q0*q3;
    q1q1 = q1*q1;
    q1q2 = q1*q2;
    q1q3 = q1*q3;
    q2q2 = q2*q2;
    q2q3 = q2*q3;
    q3q3 = q3*q3;

    aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero values into AHRS is FATAL
    if ( aMag < 0.9 ) {
        ax = ay = 0.0;
        az = 1.0;
    } else {
        // normalise the measurements
        rnorm = 1.0/aMag;
        ax *= rnorm;
        ay *= rnorm;
        az *= rnorm;
    }

    rnorm = 1.0/sqrt(Sqr(mx) + Sqr(my) + Sqr(mz));
    mx *= rnorm;
    my *= rnorm;
    mz *= rnorm;
    mx2 = 2.0 * mx;
    my2 = 2.0 * my;
    mz2 = 2.0 * mz;

    // compute reference direction of flux
    hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
    hy = mx2*(q1q2 + q0q3) + my2*(0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
    hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*(0.5 - q1q1 - q2q2);
    bx2 = 2.0*sqrt(Sqr(hx) + Sqr(hy));
    bz2 = 2.0*hz;

    // estimated direction of gravity and flux (v and w)
    vx = 2.0*(q1q3 - q0q2);
    vy = 2.0*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
    wy = bx2*(q1q2 - q0q3) + bz2*(q0q1 + q2q3);
    wz = bx2*(q0q2 + q1q3) + bz2*(0.5 - q1q1 - q2q2);

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

    // integral error scaled integral gain
    exInt += ex*MKi;
    eyInt += ey*MKi;
    ezInt += ez*MKi;

    // adjusted gyroscope measurements
    gx += MKp*ex + exInt;
    gy += MKp*ey + eyInt;
    gz += MKp*ez + ezInt;

    // integrate quaternion rate and normalise
    q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
    q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
    q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
    q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;

    // normalise quaternion
    rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
    q0 *= rnorm;
    q1 *= rnorm;
    q2 *= rnorm;
    q3 *= rnorm;

} // AHRSupdate

void  EulerAngles(void) {

    static uint8 g;

    Angle[Roll] = atan2(2.0*q2*q3 - 2.0*q0*q1 , 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
    Angle[Pitch] = asin(2.0*q1*q2 - 2.0*q0*q2);
    Angle[Yaw] = -atan2(2.0*q1*q2 - 2.0*q0*q3 ,  2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);

    for ( g = 0; g < (uint8)3; g++ ) {
#ifdef USE_GYRO_RATE
        Rate[g] = Gyro[g];
#else
        Rate[g] = ( Angle[g] - Anglep[g] ) * dTR;
        Anglep[g] = Angle[g];
#endif // USE_GYRO_RATE
    }

} // EulerAngles

/*
heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
attitude = asin(2*qx*qy + 2*qz*qw)
bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)

except when qx*qy + qz*qw = 0.5 (north pole)
which gives:
heading = 2 * atan2(x,w)
bank = 0
and when qx*qy + qz*qw = -0.5 (south pole)
which gives:
heading = -2 * atan2(x,w)
bank = 0
*/