Diff: minimu9.cpp

Revision:

1:3272ece36ce1

Parent:

0:dc35364e2291

Child:

2:85214374e094

--- a/minimu9.cpp	Thu Apr 12 13:47:23 2012 +0000
+++ b/minimu9.cpp	Mon Apr 23 14:31:08 2012 +0000
@@ -1,77 +1,284 @@
-/* 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 "mbed.h"
 #include "minimu9.h"
+#include "LSM303.h"
 #include "L3G4200D.h"
-#include "LSM303.h"
 #include "Matrix.h"
 
+minimu9::minimu9( void ) {
+    Serial pc(USBTX, USBRX);
+    t.start();
+    initialize_values();
+    gyro = new L3G4200D( p9, p10 );
+    compass = new LSM303( p9, p10 );
+
+    // 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 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;
+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);
+}
 
-    /*For debugging purposes*/
-    //OUTPUTMODE=1 will print the corrected data,
-    //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
-    OUTPUTMODE = 1;
+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***************
 
-    //#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;
+    // 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
 
-    PRINT_SERIAL = 0;
-    //PRINT_SERIAL = 1;
+    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
 
-    t.start();
-#if PRINT_SERIAL == 1
-    Serial serial(USBTX, USBRX);
+#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
-    gyro = new L3G4200D( p9, p10 );
-    compass = new LSM303( p9, p10 );
-    I2C i2c(p9, p10);
+
+    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];
+        }
+    }
+}
 
-    wait(1.5);
-    /*
-     * Give variables initial values
-     */
+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;
@@ -79,7 +286,8 @@
     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
+        //1,1,1,-1,-1,-1,1,1,1
     }, 9*sizeof(int));
     memcpy(AN_OFFSET,       (int [6]) {
         0,0,0,0,0,0
@@ -123,381 +331,21 @@
             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 DEBUG_MODE == 1
+    Serial pc(USBTX, USBRX);
+    pc.printf("!");
 
 #if PRINT_EULER == 1
-    serial.printf("ANG:");
-    serial.printf("%f, ", ToDeg(roll));
-    serial.printf("%f, ", ToDeg(pitch));
-    serial.printf("%f, ", ToDeg(yaw));
+    pc.printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), 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);
+    pc.printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] );
+    pc.printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z );
+#endif
+    pc.printf("\n");
 #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;
-}