File content as of revision 2:85214374e094:

#include <math.h>
#include "mbed.h"
#include "minimu9.h"
#include "LSM303.h"
#include "L3G4200D.h"
#include "Matrix.h"

minimu9::minimu9( void ) {
    Serial pc(USBTX, USBRX);
    t.start();
    initialize_values();
    gyro = new L3G4200D( PTE0, PTE1 );
    compass = new LSM303( PTE0, PTE1 );

    // Initiate the accel
    compass->writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz
    compass->writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale

    // Initiate the compass
    compass->init();
    compass->writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode

    // Initiate the gyro
    gyro->writeReg(L3G4200D_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
    gyro->writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale

    wait( 0.02 );

    for (int i=0; i<32; i++) { // We take some readings...
        read_gyro();
        read_accel();
        for (int y=0; y<6; y++)  // Cumulate values
            AN_OFFSET[y] += AN[y];
        wait( 0.02 );
    }

    for (int y=0; y<6; y++)
        AN_OFFSET[y] = AN_OFFSET[y]/32;

    AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

    //Serial.println("Offset:");
    for ( int y=0; y<6; y++ ) {
        pc.printf( "%d\n", AN_OFFSET[y] );
    }
    wait( 2 );

    timer=t.read_ms();
    wait(0.02);
    counter=0;
}

bool minimu9::update() { //Main Loop
    if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz
        counter++;
        timer_old = timer;
        timer=t.read_ms();
        if (timer>timer_old)
            G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
        else
            G_Dt = 0;

        // *** DCM algorithm
        // Data adquisition
        read_gyro();   // This read gyro data
        read_accel();     // Read I2C accelerometer

        if (counter > 5) { // Read compass data at 10Hz... (5 loop runs)
            counter=0;
            read_compass();    // Read I2C magnetometer
            compass_heading(); // Calculate magnetic heading
        }

        // Calculations...
        matrix_update();
        normalize();
        drift_correction();
        euler_angles();
        // ***

        print_data();
        return true;
    }
    return false;
}

void minimu9::read_gyro() {
    gyro->read();

    AN[0] = gyro->g.x;
    AN[1] = gyro->g.y;
    AN[2] = gyro->g.z;
    gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
    gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
    gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
}


// Reads x,y and z accelerometer registers
void minimu9::read_accel() {
    compass->readAcc();

    AN[3] = compass->a.x;
    AN[4] = compass->a.y;
    AN[5] = compass->a.z;
    accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
    accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
    accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}


void  minimu9::read_compass() {
    compass->readMag();

    magnetom_x = SENSOR_SIGN[6] * compass->m.x;
    magnetom_y = SENSOR_SIGN[7] * compass->m.y;
    magnetom_z = SENSOR_SIGN[8] * compass->m.z;
}

void minimu9::compass_heading() {
    float MAG_X;
    float MAG_Y;
    float cos_roll;
    float sin_roll;
    float cos_pitch;
    float sin_pitch;

    cos_roll = cos(roll);
    sin_roll = sin(roll);
    cos_pitch = cos(pitch);
    sin_pitch = sin(pitch);

    // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
    c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
    c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
    c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;

    // Tilt compensated Magnetic filed X:
    MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
    // Tilt compensated Magnetic filed Y:
    MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
    // Magnetic Heading
    MAG_Heading = atan2(-MAG_Y,MAG_X);
}

void minimu9::normalize(void) {
    float error=0;
    float temporary[3][3];
    float renorm=0;

    error= -vector_dot_product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19

    vector_scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
    vector_scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19

    vector_add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
    vector_add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19

    vector_cross_product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20

    renorm= .5 *(3 - vector_dot_product(&temporary[0][0],&temporary[0][0])); //eq.21
    vector_scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);

    renorm= .5 *(3 - vector_dot_product(&temporary[1][0],&temporary[1][0])); //eq.21
    vector_scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);

    renorm= .5 *(3 - vector_dot_product(&temporary[2][0],&temporary[2][0])); //eq.21
    vector_scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}

/**************************************************/
void minimu9::drift_correction(void) {
    float mag_heading_x;
    float mag_heading_y;
    float errorCourse;
    //Compensation the Roll, Pitch and Yaw drift.
    static float Scaled_Omega_P[3];
    static float Scaled_Omega_I[3];
    float Accel_magnitude;
    float Accel_weight;


    //*****Roll and Pitch***************

    // Calculate the magnitude of the accelerometer vector
    Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
    Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
    // Dynamic weighting of accelerometer info (reliability filter)
    // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
    Accel_weight = 1 - 2*abs(1 - Accel_magnitude),0,1;  //
    if( 1 < Accel_weight ) {
        Accel_weight = 1;
    }
    else if( 0 > Accel_weight ) {
        Accel_weight = 0;
    }

    vector_cross_product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
    vector_scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);

    vector_scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
    vector_add(Omega_I,Omega_I,Scaled_Omega_I);

    //*****YAW***************
    // We make the gyro YAW drift correction based on compass magnetic heading

    mag_heading_x = cos(MAG_Heading);
    mag_heading_y = sin(MAG_Heading);
    errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
    vector_scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.

    vector_scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
    vector_add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.

    vector_scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
    vector_add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}


void minimu9::matrix_update(void) {
    Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
    Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
    Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw

    Accel_Vector[0]=accel_x;
    Accel_Vector[1]=accel_y;
    Accel_Vector[2]=accel_z;

    vector_add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
    vector_add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

#if OUTPUTMODE==1
    Update_Matrix[0][0]=0;
    Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
    Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
    Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
    Update_Matrix[1][1]=0;
    Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
    Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
    Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
    Update_Matrix[2][2]=0;
#else                    // Uncorrected data (no drift correction)
    Update_Matrix[0][0]=0;
    Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
    Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
    Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
    Update_Matrix[1][1]=0;
    Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
    Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
    Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
    Update_Matrix[2][2]=0;
#endif

    matrix_multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c

    for (int x=0; x<3; x++) { //Matrix Addition (update)
        for (int y=0; y<3; y++) {
            DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
        }
    }
}

void minimu9::euler_angles(void) {
    pitch = -asin(DCM_Matrix[2][0]);
    roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
    yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}


float minimu9::get_roll( void ) {
    return ToDeg( roll );
}
float minimu9::get_pitch( void ) {
    return ToDeg( pitch );
}
float minimu9::get_yaw( void ) {
    return ToDeg( yaw );
}


void minimu9::initialize_values( void ) {
    G_Dt=0.02;
    timer=0;
    timer24=0;
    counter=0;
    gyro_sat=0;

    memcpy(SENSOR_SIGN, (int [9]) {
        1,-1,-1,-1,1,1,1,-1,-1
        //1,1,1,-1,-1,-1,1,1,1
    }, 9*sizeof(int));
    memcpy(AN_OFFSET,       (int [6]) {
        0,0,0,0,0,0
    }, 6*sizeof(int));
    memcpy(Accel_Vector,    (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(Gyro_Vector,     (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(Omega_Vector,    (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(Omega_P,         (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(Omega_I,         (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(Omega,           (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(errorRollPitch,  (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy(errorYaw,        (float [3]) {
        0,0,0
    }, 3*sizeof(float));
    memcpy( DCM_Matrix,     (float[3][3]) {
        {
            1,0,0
        }, { 0,1,0 }, { 0,0,1 }
    }, 9*sizeof(float) );
    memcpy( Update_Matrix,  (float[3][3]) {
        {
            0,1,2
        }, { 3,4,5 }, { 6,7,8 }
    }, 9*sizeof(float) );
    memcpy( Temporary_Matrix, (float[3][3]) {
        {
            0,0,0
        }, { 0,0,0 }, { 0,0,0 }
    }, 9*sizeof(float) );
}

void minimu9::print_data(void) {
#if DEBUG_MODE == 1
//    Serial pc(USBTX, USBRX);
    printf("!");

#if PRINT_EULER == 1
    printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), ToDeg(yaw));
#endif
#if PRINT_ANALOGS==1
    printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] );
    printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z );
#endif
    printf("\n");
#endif

}