File content as of revision 0:dc35364e2291:

/* mbed L3G4200D Library version 0.1
 * Copyright (c) 2012 Prediluted
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
 
/*
 * Communication for the minimu9, ported from the original Polulu minIMU-9 Arduino code
 */
#include <stdlib.h>
#include <algorithm>
#include <iostream>
#include <vector>
#include <math.h>
#include "minimu9.h"
#include "L3G4200D.h"
#include "LSM303.h"
#include "Matrix.h"


/*
 * void setup() replaced with the constructor
 */
minimu9::minimu9() {
    // Calibration constants defaults, will need to calibrate then use
    // set_calibration_constants() to have the proper values for your board
    M_X_MIN = -570;
    M_Y_MIN = -746;
    M_Z_MIN = -416;
    M_X_MAX = 477;
    M_Y_MAX = 324;
    M_Z_MAX = 652;

    /*For debugging purposes*/
    //OUTPUTMODE=1 will print the corrected data,
    //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
    OUTPUTMODE = 1;

    //#define PRINT_DCM 0     //Will print the whole direction cosine matrix
    PRINT_ANALOGS = 0; //Will print the analog raw data
    PRINT_EULER = 1;   //Will print the Euler angles Roll, Pitch and Yaw
    PRINT_DCM = 0;

    PRINT_SERIAL = 0;
    //PRINT_SERIAL = 1;

    t.start();
#if PRINT_SERIAL == 1
    Serial serial(USBTX, USBRX);
#endif
    gyro = new L3G4200D( p9, p10 );
    compass = new LSM303( p9, p10 );
    I2C i2c(p9, p10);

    wait(1.5);
    /*
     * Give variables initial values
     */
    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
    }, 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) );

    /*
     * Initialize the accel, compass, and gyro
     */
    // Accel
    compass->accRegisterWrite(LSM303::ACC_CTRL_REG1, 0x24); // normal power mode, all axes enabled, 50 Hz
    compass->accRegisterWrite(LSM303::ACC_CTRL_REG4, 0x30 ); // 8g full scale
    // Compass
    compass->magRegisterWrite(LSM303::MAG_MR_REG, 0x00); // continuous conversion mode
    // Gyro
    gyro->registerWrite(L3G4200D::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
    gyro->registerWrite(L3G4200D::CTRL_REG4, 0x20); // 2000 dps full scale

#if PRINT_SERIAL == 1
    serial.printf( "initiatied\n" );
}
#endif

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.2);
}

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

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

#if PRINT_SERIAL == 1
serial.printf("Offset:\n");
for (int y=0; y<6; y++) {
    serial.printf( "%d", AN_OFFSET[y] );
}
serial.printf( "\n" );
#endif
wait(2);
timer=t.read_ms();
wait(0.2);
counter=0;

#if PRINT_SERIAL == 1
while ( 1 ) {
    get_data();
}
#endif
}

void minimu9::get_data() { //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();
        // ***

        if ( 1 == PRINT_SERIAL ) {
            print_data();
        }
    }

}

long convert_to_dec(float x) {
    return x*10000000;
}

void minimu9::Read_Gyro() {
    gyro->read();
    std::vector<short> g = gyro->read();
    AN[0] = g[0];
    AN[1] = g[1];
    AN[2] = g[2];

    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() {
    std::vector<short> a = compass->accRead();

    AN[3] = a[0];
    AN[4] = a[1];
    AN[5] = a[2];

    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::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.0;
    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::Read_Compass() {
    std::vector<short> m = compass->magRead();

    magnetom_x = SENSOR_SIGN[6] * m[0];
    magnetom_y = SENSOR_SIGN[7] * m[1];
    magnetom_z = SENSOR_SIGN[8] * m[2];
}

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 );
    if ( Accel_weight < 0 ) {
        Accel_weight = 0;
    } else if ( Accel_weight > 1 ) {
        Accel_weight = 1;
    }

    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);//.01 proportional of YAW.
    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.

    Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator
    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]);
}

void minimu9::print_data(void) {
    Serial serial(USBTX, USBRX);

    serial.printf("!");

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

#if PRINT_DCM == 1
    serial.printf (",DCM:");
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][0]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][1]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][2]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][0]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][1]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][2]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][0]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][1]));
    serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][2]));
#endif
    serial.printf("\n");
}

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

int min ( int a, int b ) {
    if ( b < a )
        return b;
    return a;
}
int max ( int a, int b ) {
    if ( b > a )
        return b;
    return a;
}

void minimu9::Calibrate_compass( void ) {
    Serial serial(USBTX, USBRX);
    int running_min[3] = {2047, 2047, 2047}, running_max[3] = {-2048, -2048, -2048};
    while ( 1 ) {
        std::vector<short> m = compass->magRead();

        for ( int i = 0; i < 3; i++ ) {
            running_min[i] = min(running_min[i], m[i]);
            running_max[i] = max(running_max[i], m[i]);
        }

        serial.printf("M min ");
        serial.printf("X: %d", (int)running_min[0] );
        serial.printf(" Y: %d", (int)running_min[1]);
        serial.printf(" Z: %d", (int)running_min[2]);

        serial.printf(" M max ");
        serial.printf("X: %d", (int)running_max[0] );
        serial.printf(" Y: %d", (int)running_max[1]);
        serial.printf(" Z: %d",(int)running_max[2]);
        serial.printf("\n");

    }
}

void minimu9::set_calibration_values( int x_min, int y_min, int z_min, int x_max, int y_max, int z_max ) {
    M_X_MIN = x_min;
    M_Y_MIN = y_min;
    M_Z_MIN = z_min;
    M_X_MAX = x_max;
    M_Y_MAX = y_max;
    M_Z_MAX = z_max;
}

void minimu9::set_print_settings( int mode, int analogs, int euler, int dcm, int serial ) {
    /*For debugging purposes*/
    //OUTPUTMODE=1 will print the corrected data,
    //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
    OUTPUTMODE = mode;

    //#define PRINT_DCM 0     //Will print the whole direction cosine matrix
    PRINT_ANALOGS = analogs; //Will print the analog raw data
    PRINT_EULER = euler;   //Will print the Euler angles Roll, Pitch and Yaw
    PRINT_DCM = dcm;

    PRINT_SERIAL = serial;
    //PRINT_SERIAL = 1;
}