Revision 20:e116e596e540, committed 2012-11-05

Comitter:

maetugr

Date:

Mon Nov 05 09:19:01 2012 +0000

Parent:

19:40c252b4a792

Child:

21:c2a2e7cbabdd

Commit message:

Eine Achse stabil! bereits abgehoben an Schnur! (erst Gyro im I2C_Sensor)(acos nan abgefangen)

Changed in this revision

--- a/RC/RC_Channel.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/RC/RC_Channel.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -25,7 +25,7 @@
     timer.stop();
     int tester = timer.read_us();
     if(tester >= 1000 && tester <=2000)
-        time = tester;
+        time = (tester-70);  // TODO: skalierung mit calibrierung (speichern....)
     timer.reset();
     timer.start();
 }

--- a/Sensors/Acc/ADXL345.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Acc/ADXL345.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -1,47 +1,97 @@
 #include "ADXL345.h"
+#include "mbed.h"
+
+ADXL345::ADXL345(PinName sda, PinName scl) : i2c(sda, scl) {
 
-ADXL345::ADXL345(PinName sda, PinName scl) : I2C_Sensor(sda, scl, ADXL345_I2C_ADDRESS)
-{
+    //400kHz, allowing us to use the fastest data rates.
+    //there are other chips on board, sorry
+    i2c.frequency(400000);   
     // initialize the BW data rate
-    writeRegister(ADXL345_BW_RATE_REG, ADXL345_1600HZ); //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
+    char tx[2];
+    tx[0] = ADXL345_BW_RATE_REG;
+    tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register 
+    i2c.write( ADXL345_WRITE , tx, 2);  
 
     //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
-    writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B);       // full res and +_16g
+   
+ char rx[2];
+    rx[0] = ADXL345_DATA_FORMAT_REG;
+    rx[1] = 0x0B; 
+     // full res and +_16g
+ i2c.write( ADXL345_WRITE , rx, 2); 
  
     // Set Offset  - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
-    writeRegister(ADXL345_OFSX_REG, 0x00);              // TODO: 0xFD (sein offset! brauch ich auch?)
-    writeRegister(ADXL345_OFSY_REG, 0x00);              // y[1] = 0x03;
-    writeRegister(ADXL345_OFSZ_REG, 0x00);              // z[1] = 0xFE;
+  char x[2];
+    x[0] = ADXL345_OFSX_REG ;
+    //x[1] = 0xFD; 
+    x[1] = 0x00; 
+ i2c.write( ADXL345_WRITE , x, 2);
+  char y[2];
+    y[0] = ADXL345_OFSY_REG ;
+    //y[1] = 0x03;
+    y[1] = 0x00; 
+ i2c.write( ADXL345_WRITE , y, 2);
+ char z[2];
+    z[0] = ADXL345_OFSZ_REG ;
+    //z[1] = 0xFE;
+    z[1] = 0x00;
+ i2c.write( ADXL345_WRITE , z, 2);
  
-    writeRegister(ADXL345_POWER_CTL_REG, 0x00);         // set power control
-    writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B);       // set data format
-    setDataRate(ADXL345_3200HZ);                        // set data rate
-    writeRegister(ADXL345_POWER_CTL_REG, 0x08);         // set mode
+ // MY INITIALISATION -------------------------------------------------------
+ 
+    writeReg(ADXL345_POWER_CTL_REG, 0x00); // set power control
+    writeReg(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format
+    setDataRate(ADXL345_3200HZ); // set data rate
+    writeReg(ADXL345_POWER_CTL_REG, 0x08); // set mode
 }
 
-void ADXL345::read()
-{
-    char buffer[6];                                     // 8-Bit pieces of axis data
+void ADXL345::read(){
+    char buffer[6];    
+    readMultiReg(ADXL345_DATAX0_REG, buffer, 6);
     
-    readMultiRegister(ADXL345_DATAX0_REG, buffer, 6);   // read axis registers using I2C
-    
-    data[0] = (float)(short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
-    data[1] = (float)(short) (buffer[3] << 8 | buffer[2]);
-    data[2] = (float)(short) (buffer[5] << 8 | buffer[4]);
+    data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
+    data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
+    data[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
     
     // calculate the angles for roll and pitch (0,1)
-    float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));  // calculate the absolute of the magnetic field vector
-    angle[0] = -(RAD2DEG * acos((float)data[1] / R)-90);                                    // roll  - angle of magnetic field vector in x direction
-    angle[1] =   RAD2DEG * acos((float)data[0] / R)-90;                                     // pitch - angle of magnetic field vector in y direction
-    angle[2] =   RAD2DEG * acos((float)data[2] / R);                                        // angle from magnetic field vector in direction which it has
+    float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
+    float temp[3];
+    
+    temp[0] = -(Rad2Deg * acos((float)data[1] / R)-90);
+    temp[1] =   Rad2Deg * acos((float)data[0] / R)-90;
+    temp[2] =   Rad2Deg * acos((float)data[2] / R);
+    
+    for(int i = 0;i < 3; i++)
+        if (temp[i] > -360 && temp[i] < 360)
+            angle[i] = temp[i];
 }
 
-void ADXL345::setDataRate(char rate)
-{
-    char registerContents = readRegister(ADXL345_BW_RATE_REG); // get the current register contents, so we don't clobber the power bit
+void ADXL345::writeReg(char address, char data){ 
+   char tx[2];
+   tx[0] = address;
+   tx[1] = data;
+   i2c.write(ADXL345_WRITE, tx, 2);
+}
+
+char ADXL345::readReg(char address){   
+   char tx = address;
+   char output; 
+    i2c.write( ADXL345_WRITE , &tx, 1);  //tell it what you want to read
+    i2c.read( ADXL345_READ , &output, 1);    //tell it where to store the data
+    return output; 
+}
+
+void ADXL345::readMultiReg(char address, char* output, int size) {
+    i2c.write(ADXL345_WRITE, &address, 1, true); //tell it where to read from
+    i2c.read(ADXL345_READ , output, size, true); //tell it where to store the data read
+}
+
+void ADXL345::setDataRate(char rate) {
+    //Get the current register contents, so we don't clobber the power bit.
+    char registerContents = readReg(ADXL345_BW_RATE_REG);
 
     registerContents &= 0x10;
     registerContents |= rate;
 
-    writeRegister(ADXL345_BW_RATE_REG, registerContents);
+    writeReg(ADXL345_BW_RATE_REG, registerContents);
 }
\ No newline at end of file

--- a/Sensors/Acc/ADXL345.h	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Acc/ADXL345.h	Mon Nov 05 09:19:01 2012 +0000
@@ -4,45 +4,38 @@
 #define ADXL345_H
 
 #include "mbed.h"
-#include "I2C_Sensor.h"
-
-#define ADXL345_I2C_ADDRESS         0xA6
-// read or write bytes
-//#define ADXL345_READ    0xA7  
-//#define ADXL345_WRITE   0xA6 
-//#define ADXL345_ADDRESS 0x53
 
 // register addresses
-#define ADXL345_DEVID_REG           0x00
-#define ADXL345_THRESH_TAP_REG      0x1D
-#define ADXL345_OFSX_REG            0x1E
-#define ADXL345_OFSY_REG            0x1F
-#define ADXL345_OFSZ_REG            0x20
-#define ADXL345_DUR_REG             0x21
-#define ADXL345_LATENT_REG          0x22
-#define ADXL345_WINDOW_REG          0x23
-#define ADXL345_THRESH_ACT_REG      0x24
-#define ADXL345_THRESH_INACT_REG    0x25
-#define ADXL345_TIME_INACT_REG      0x26
-#define ADXL345_ACT_INACT_CTL_REG   0x27
-#define ADXL345_THRESH_FF_REG       0x28
-#define ADXL345_TIME_FF_REG         0x29
-#define ADXL345_TAP_AXES_REG        0x2A
-#define ADXL345_ACT_TAP_STATUS_REG  0x2B
-#define ADXL345_BW_RATE_REG         0x2C
-#define ADXL345_POWER_CTL_REG       0x2D
-#define ADXL345_INT_ENABLE_REG      0x2E
-#define ADXL345_INT_MAP_REG         0x2F
-#define ADXL345_INT_SOURCE_REG      0x30
-#define ADXL345_DATA_FORMAT_REG     0x31
-#define ADXL345_DATAX0_REG          0x32
-#define ADXL345_DATAX1_REG          0x33
-#define ADXL345_DATAY0_REG          0x34
-#define ADXL345_DATAY1_REG          0x35
-#define ADXL345_DATAZ0_REG          0x36
-#define ADXL345_DATAZ1_REG          0x37
-#define ADXL345_FIFO_CTL            0x38
-#define ADXL345_FIFO_STATUS         0x39
+#define ADXL345_DEVID_REG          0x00
+#define ADXL345_THRESH_TAP_REG     0x1D
+#define ADXL345_OFSX_REG           0x1E
+#define ADXL345_OFSY_REG           0x1F
+#define ADXL345_OFSZ_REG           0x20
+#define ADXL345_DUR_REG            0x21
+#define ADXL345_LATENT_REG         0x22
+#define ADXL345_WINDOW_REG         0x23
+#define ADXL345_THRESH_ACT_REG     0x24
+#define ADXL345_THRESH_INACT_REG   0x25
+#define ADXL345_TIME_INACT_REG     0x26
+#define ADXL345_ACT_INACT_CTL_REG  0x27
+#define ADXL345_THRESH_FF_REG      0x28
+#define ADXL345_TIME_FF_REG        0x29
+#define ADXL345_TAP_AXES_REG       0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG        0x2C
+#define ADXL345_POWER_CTL_REG      0x2D
+#define ADXL345_INT_ENABLE_REG     0x2E
+#define ADXL345_INT_MAP_REG        0x2F
+#define ADXL345_INT_SOURCE_REG     0x30
+#define ADXL345_DATA_FORMAT_REG    0x31
+#define ADXL345_DATAX0_REG         0x32
+#define ADXL345_DATAX1_REG         0x33
+#define ADXL345_DATAY0_REG         0x34
+#define ADXL345_DATAY1_REG         0x35
+#define ADXL345_DATAZ0_REG         0x36
+#define ADXL345_DATAZ1_REG         0x37
+#define ADXL345_FIFO_CTL           0x38
+#define ADXL345_FIFO_STATUS        0x39
 
 // data rate codes
 #define ADXL345_3200HZ      0x0F
@@ -56,6 +49,11 @@
 #define ADXL345_12HZ5       0x07
 #define ADXL345_6HZ25       0x06
 
+// read or write bytes
+#define ADXL345_READ    0xA7  
+#define ADXL345_WRITE   0xA6 
+#define ADXL345_ADDRESS 0x53
+
 //the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
 //when ALT ADDRESS pin is high:
 //#define ADXL345_READ    0x3B   
@@ -66,17 +64,24 @@
 #define ADXL345_Y           0x01
 #define ADXL345_Z           0x02
 
-#define RAD2DEG         57.295779513082320876798154814105
+#define Rad2Deg         57.295779513082320876798154814105
 
-class ADXL345 : public I2C_Sensor
+typedef char byte;
+
+class ADXL345
 {
     public:
-        ADXL345(PinName sda, PinName scl);  // constructor, uses I2C_Sensor
-        virtual void read();                // read all axis from register to array data
-        float angle[3];                     // where the calculated angles are stored
+        ADXL345(PinName sda, PinName scl); // constructor, uses i2c
+        void read(); // read all axis to array
+        int data[3]; // where the measured data is saved
+        float angle[3]; // where the calculated angles are stored
        
     private:
-        void setDataRate(char rate);        // data rate configuration (not only a reg to write)
+        I2C i2c; // i2c object to communicate
+        void writeReg(byte reg, byte value); // write one single register to sensor
+        byte readReg(byte reg); // read one single register from sensor
+        void readMultiReg(char startAddress, char* ptr_output, int size); // read multiple regs
+        void setDataRate(char rate); // data rate configuration (not only a reg to write)
 };
 
 #endif

--- a/Sensors/Alt/BMP085.h	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Alt/BMP085.h	Mon Nov 05 09:19:01 2012 +0000
@@ -22,7 +22,7 @@
     private:
         // raw data and function to measure it
         int raw[3];
-        void readraw();
+        //void readraw();
         
         // calibration parameters and their saving
         int Min[3];

--- a/Sensors/Comp/HMC5883.h	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Comp/HMC5883.h	Mon Nov 05 09:19:01 2012 +0000
@@ -23,7 +23,7 @@
         float get_angle();
          
     private:
-        void readraw();                 // function to get raw data
+        virtual void readraw();                 // function to get raw data
         
         float scale[3];                 // calibration parameters
         float offset[3];

--- a/Sensors/Gyro/L3G4200D.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -35,9 +35,9 @@
     
     const int count = 50;
     for (int i = 0; i < count; i++) {                   // read 50 times the data in a very short time
-        read();
+        readraw();
         for (int j = 0; j < 3; j++)
-            Gyro_calib[j] += data[j];
+            Gyro_calib[j] += raw[j];
         wait(0.001);          // TODO: maybe less or no wait !!
     }
     
@@ -47,21 +47,24 @@
 
 void L3G4200D::read()
 {
-    char buffer[6];                                     // 8-Bit pieces of axis data
-    
-    //buffer[0] = L3G4200D_OUT_X_L | (1 << 7);          // TODO: wiiiiiiso?!
-    
-    readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C
+    readraw();                                          // read raw measurement data
     
-    data[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
-    data[1] = (short) (buffer[3] << 8 | buffer[2]);
-    data[2] = (short) (buffer[5] << 8 | buffer[4]);
-    
-    for (int j = 0; j < 3; j++)
-            data[j] -= offset[j];                       // add offset from calibration
+    for (int i = 0; i < 3; i++)
+            data[i] = raw[i] - offset[i];               // subtract offset from calibration
 }
 
 int L3G4200D::readTemp()
 {
     return (short) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
+}
+
+void L3G4200D::readraw()
+{
+    char buffer[6];                                     // 8-Bit pieces of axis data
+    
+    readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: wiiiiiiso?!   | (1 << 7)
+    
+    raw[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
+    raw[1] = (short) (buffer[3] << 8 | buffer[2]);
+    raw[2] = (short) (buffer[5] << 8 | buffer[4]);
 }
\ No newline at end of file

--- a/Sensors/Gyro/L3G4200D.h	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.h	Mon Nov 05 09:19:01 2012 +0000
@@ -6,7 +6,7 @@
 #include "mbed.h"
 #include "I2C_Sensor.h"
 
-#define L3G4200D_I2C_ADDRESS    0xD0               // TODO: Adressen???
+#define L3G4200D_I2C_ADDRESS    0xD0
 
 // register addresses
 #define L3G4200D_WHO_AM_I       0x0F
@@ -49,6 +49,7 @@
         
     private:
         float offset[3];                    // offset that's subtracted from every measurement
+        virtual void readraw();
 };
 
 #endif
\ No newline at end of file

--- a/Sensors/I2C_Sensor.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/I2C_Sensor.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -7,8 +7,8 @@
 I2C_Sensor::I2C_Sensor(PinName sda, PinName scl, int8_t i2c_address) :  i2c(sda, scl), local("local")
 {
     I2C_Sensor::i2c_address = i2c_address;
-    //i2c.frequency(400000); // standard speed
-    i2c.frequency(1500000); // ultrafast!
+    i2c.frequency(400000); // standard speed
+    //i2c.frequency(1500000); // ultrafast!
 }
 
 void I2C_Sensor::saveCalibrationValues(float values[], int size, char * filename)

--- a/Sensors/I2C_Sensor.h	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/I2C_Sensor.h	Mon Nov 05 09:19:01 2012 +0000
@@ -26,7 +26,7 @@
         
         // raw data and function to measure it
         int raw[3];
-        //virtual void readraw() = 0;
+        virtual void readraw() = 0;
         
     private:
         I2C i2c;            // I2C-Bus

--- a/main.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/main.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -13,9 +13,9 @@
 #define RAD2DEG         57.295779513082320876798154814105   // ratio between radians and degree (360/2Pi)
 #define RATE            0.02                                // speed of the interrupt for Sensors and PID
 
-#define P_VALUE         0.1                                // viel zu tief!!!!!
-#define I_VALUE         0.0                                // 
-#define D_VALUE         0.0                                // 
+#define P_VALUE         0.05                                // PID values
+#define I_VALUE         5  
+#define D_VALUE         0.015
 
 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
 
@@ -38,12 +38,14 @@
 
 
 // global varibles
+bool            armed = false;          // this variable is for security
 unsigned long   dt_get_data = 0; // TODO: dt namen
 unsigned long   time_get_data = 0;
 unsigned long   dt_read_sensors = 0;
 unsigned long   time_read_sensors = 0;
 float           angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
 float           tempangle = 0; // temporärer winkel für yaw ohne kompass
+float           Gyro_angle[3] ={0,0,0};
 float co; // PID test
 
 void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
@@ -62,29 +64,51 @@
     dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop
     time_get_data = GlobalTimer.read_us();      // set new time for next measurement
     
+    Gyro_angle[0] += Gyro.data[0] *dt_get_data/15000000.0;
+    Gyro_angle[1] += Gyro.data[1] *dt_get_data/15000000.0;
+    Gyro_angle[2] += Gyro.data[2] *dt_get_data/15000000.0;
+    
     // calculate angles for roll, pitch an yaw
     angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
     angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen
     tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
-    angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here
+    angle[2] = Gyro_angle[2]; // gyro only here
     
     // PID controlling
     Controller.setProcessValue(angle[1]);
     
+    // Aming/ disarming
+    if(RC[2].read() < 1020 && RC[3].read() < 1020)
+        armed = false;
+    if(RC[2].read() < 500 || RC[1].read() < 500 || RC[0].read() < 500)
+        armed = false;
+    if(RC[2].read() < 1020 && RC[3].read() > 1850)
+        armed = true;
+    
     // calculate new motorspeeds
     co = Controller.compute() - 1000;
-    if (RC[2].read() > 1100) // zu SICHERHEIT! zum testen (später ersetzen armed unarmed)
+    if (armed) // zur SICHERHEIT!
     {
+        #if 0
+            Motor[0] = RC[2].read();
+            Motor[1] = RC[2].read();
+            Motor[2] = RC[2].read();
+            Motor[3] = RC[2].read();
+        #else
+            Motor[0] = RC[2].read()+co;
+            Motor[2] = RC[2].read()-co;
+        #endif
+        
         /*Motor[0] = RC[2].read()+((RC[0].read() - 1500)/10.0)+40;
         Motor[2] = RC[2].read()-((RC[0].read() - 1500)/10.0)-40;*/
-        Motor[0] = RC[2].read()+co+40;
-        Motor[2] = RC[2].read()-co-40;
+        /**/
     } else {
         Motor[0] = 1000;
         Motor[1] = 1000;
         Motor[2] = 1000;
         Motor[3] = 1000;
-    }
+    }    
+    
     /*Motor[0] = 1000 + (100 - (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000; // test für erste reaktion der motoren entgegen der Auslenkung
     Motor[1] = 1000 + (100 - (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;
     Motor[2] = 1000 + (100 + (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000;
@@ -117,24 +141,35 @@
     Datagetter.attach(&get_Data, RATE);     // start to get data all RATEms
     
     while(1) { 
-        pc.locate(10,5); // PC output
+        pc.locate(30,0); // PC output
         pc.printf("dt:%dms   dt_sensors:%dus    Altitude:%6.1fm   ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
-        pc.locate(10,8);
-        pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
-        pc.locate(10,9);
-        pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
-        pc.locate(10,10);
+        pc.locate(5,1);
+        if(armed)
+            pc.printf("ARMED!!!!!!!!!!!!!");
+        else
+            pc.printf("DIS_ARMED            ");
+        pc.locate(5,3);
+        pc.printf("Roll:%6.1f   Pitch:%6.1f   Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
+        
+        pc.locate(5,5);
+        pc.printf("Gyro.data: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
+        pc.locate(5,6);
+        pc.printf("Gyro_angle: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro_angle[0], Gyro_angle[1], Gyro_angle[2]);
+        
+        pc.locate(5,8);
+        pc.printf("Acc.data: X:%6d  Y:%6d  Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); 
+        pc.locate(5,9);
+        pc.printf("Acc.angle: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
+        
+        pc.locate(5,11);
+        pc.printf("PID Test: %6.1f", co);
+        
+        pc.locate(10,15);
         pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp.get_angle(), tempangle);
-        pc.locate(10,12);
+        pc.locate(10,16);
         pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
-        pc.locate(10,13);
+        pc.locate(10,17);
         //pc.printf("Comp_scale: %6.4f %6.4f %6.4f   ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private
-        pc.locate(10,15);
-        pc.printf("PID Test: %6.1f", co);
-        pc.locate(10,16);
-        pc.printf("Gyro_data: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
-        pc.locate(10,17);
-        pc.printf("Acc_data: X:%6.1f  Y:%6.1f  Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]);
         pc.locate(10,18);
         pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());