Revision 14:cf260677ecde, committed 2012-10-27

Comitter:

maetugr

Date:

Sat Oct 27 10:53:43 2012 +0000

Parent:

13:4737ee9ebfee

Child:

15:753c5d6a63b3

Commit message:

I2C Workaround gefunden!!! erster Test geklappt, vor Umschreibung

Changed in this revision

--- a/BMP085/BMP085.cpp	Thu Oct 25 19:27:56 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,150 +0,0 @@
-
-#include "mbed.h"
-#include "BMP085.h"
-
-//I2C Adresse
-#define BMP085_ADRESS 0xEE
-
-#define xpow(x, y) ((long)1 << y)
-
-
-// Constructor
-// -----------------------------------------------
-BMP085::BMP085(PinName sda, PinName scl) : i2c_(sda, scl)
-    {
-    Init();
-    // MYINIT -------
-    oss = 0; //Oversampling des Barometers setzen
-    // MYINIT -------
-    }
-    
-
-// Temperatur und Druck auslesen und berechnen
-// -----------------------------------------------
-void BMP085::Update () 
-    {
-    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
-    unsigned long B4, B7;
-
-    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
-    // Wait at least 4.5ms
-    wait(0.005);
-    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
-    
-    X1 = ((UTemp - AC6) * AC5) >> 15;
-    X2 = (MC << 11) / (X1 + MD);
-    B5 = X1 + X2;
-    Temperature = (float)((B5 + 8) >> 4)/10.0;
-
-    twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
-    // Wait at least 4.5ms
-    wait(0.005);
-    UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
-
-    B6 = B5 - 4000;
-    X1 = (B2 * (B6 * B6) >> 12) >> 11;
-    X2 = (AC2 * B6) >> 11;
-    X3 = X1 + X2;
-    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
-
-    X1 = (AC3 * B6) >> 13;
-    X2 = (B1 * (B6 * B6) >> 12) >> 16;
-    X3 = ((X1 + X2) + 2) >> 2;
-    B4 = AC4 * (X3 + 32768) >> 15;
-    
-    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
-    
-    if (B7 < 0x80000000) 
-        {
-        P = (2 * B7) / B4;
-        }
-    else 
-        {
-        P = 2* (B7 / B4);
-        }
-    X1 = (P >> 8) * (P >> 8);
-    X1 = (X1 * 3038) >> 16;
-    X2 = (-7357 * P) >> 16;    
-    P = P + ((X1 + X2 + 3791) >> 4);
-    Pressure = (float)P / 100.0;
-    }
-
-
-// Hoehe u.M. berechnen  (Druck in hPa)
-// -----------------------------------------------
-float BMP085::CalcAltitude(float Press)
-    {
-    float A = Press/1013.25;
-    float B = 1/5.25588;
-    float C = pow(A,B);
-    
-    C = 1 - C;
-    C = C / 22.5577e-6;
-    return C;
-    }
-    
-        
-// Drucksensor initialisieren  
-// -----------------------------------------------
-void BMP085::Init () 
-    {
-    AC1 = twi_readshort(BMP085_ADRESS, 0xaa);
-    AC2 = twi_readshort(BMP085_ADRESS, 0xac);
-    AC3 = twi_readshort(BMP085_ADRESS, 0xae);
-    AC4 = twi_readshort(BMP085_ADRESS, 0xb0);
-    AC5 = twi_readshort(BMP085_ADRESS, 0xb2);
-    AC6 = twi_readshort(BMP085_ADRESS, 0xb4);
-    B1 = twi_readshort(BMP085_ADRESS, 0xb6);
-    B2 = twi_readshort(BMP085_ADRESS, 0xb8);
-    MB = twi_readshort(BMP085_ADRESS, 0xba);
-    MC = twi_readshort(BMP085_ADRESS, 0xbc);
-    MD = twi_readshort(BMP085_ADRESS, 0xbe);
-    }
-    
-    
-// -----------------------------------------------
-unsigned short BMP085::twi_readshort (int id, int addr) {
-    unsigned short i;
-
-    i2c_.start();
-    i2c_.write(id);
-    i2c_.write(addr);
-
-    i2c_.start();
-    i2c_.write(id | 1);
-    i = i2c_.read(1) << 8;
-    i |= i2c_.read(0);
-    i2c_.stop();
-
-    return i;
-}
-
-
-// -----------------------------------------------
-unsigned long BMP085::twi_readlong (int id, int addr) {
-    unsigned long i;
-
-    i2c_.start();
-    i2c_.write(id);
-    i2c_.write(addr);
-
-    i2c_.start();
-    i2c_.write(id | 1);
-    i = i2c_.read(1) << 16;
-    i |= i2c_.read(1) << 8;
-    i |= i2c_.read(0);
-    i2c_.stop();
-
-    return i;
-}
-
-
-// -----------------------------------------------
-void BMP085::twi_writechar (int id, int addr, int dat) {
-
-    i2c_.start();
-    i2c_.write(id);
-    i2c_.write(addr);
-    i2c_.write(dat);
-    i2c_.stop();
-}

--- a/BMP085/BMP085.h	Thu Oct 25 19:27:56 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
- 
-#ifndef BMP085_I2C_H
-#define BMP085_I2C_H
-
-#include "mbed.h"
-
-class BMP085
-    {
-    private:
-        I2C i2c_;
-        
-    public:
-        BMP085(PinName sda, PinName scl);
-    
-        void    Update();
-        float   Temperature;
-        float   Pressure;
-        float   CalcAltitude(float Press);
-        short   oss;
-    
-    protected:
-        void Init();
-        unsigned short twi_readshort (int, int);
-        unsigned long twi_readlong (int, int);
-        void twi_writechar (int, int, int);
-    
-    
-    private:
-    
-        short AC1, AC2, AC3, B1, B2, MB, MC, MD;
-        unsigned short AC4, AC5, AC6;
-    };
-
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085_old.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,150 @@
+
+#include "mbed.h"
+#include "BMP085_old.h"
+
+//I2C Adresse
+#define BMP085_ADRESS 0xEE
+
+#define xpow(x, y) ((long)1 << y)
+
+
+// Constructor
+// -----------------------------------------------
+BMP085_old::BMP085_old(PinName sda, PinName scl) : i2c_(sda, scl)
+    {
+    Init();
+    // MYINIT -------
+    oss = 0; //Oversampling des Barometers setzen
+    // MYINIT -------
+    }
+    
+
+// Temperatur und Druck auslesen und berechnen
+// -----------------------------------------------
+void BMP085_old::Update () 
+    {
+    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+    unsigned long B4, B7;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
+    // Wait at least 4.5ms
+    wait(0.005);
+    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+    
+    X1 = ((UTemp - AC6) * AC5) >> 15;
+    X2 = (MC << 11) / (X1 + MD);
+    B5 = X1 + X2;
+    Temperature = (float)((B5 + 8) >> 4)/10.0;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
+    // Wait at least 4.5ms
+    wait(0.005);
+    UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
+
+    B6 = B5 - 4000;
+    X1 = (B2 * (B6 * B6) >> 12) >> 11;
+    X2 = (AC2 * B6) >> 11;
+    X3 = X1 + X2;
+    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
+
+    X1 = (AC3 * B6) >> 13;
+    X2 = (B1 * (B6 * B6) >> 12) >> 16;
+    X3 = ((X1 + X2) + 2) >> 2;
+    B4 = AC4 * (X3 + 32768) >> 15;
+    
+    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+    
+    if (B7 < 0x80000000) 
+        {
+        P = (2 * B7) / B4;
+        }
+    else 
+        {
+        P = 2* (B7 / B4);
+        }
+    X1 = (P >> 8) * (P >> 8);
+    X1 = (X1 * 3038) >> 16;
+    X2 = (-7357 * P) >> 16;    
+    P = P + ((X1 + X2 + 3791) >> 4);
+    Pressure = (float)P / 100.0;
+    }
+
+
+// Hoehe u.M. berechnen  (Druck in hPa)
+// -----------------------------------------------
+float BMP085_old::CalcAltitude(float Press)
+    {
+    float A = Press/1013.25;
+    float B = 1/5.25588;
+    float C = pow(A,B);
+    
+    C = 1 - C;
+    C = C / 22.5577e-6;
+    return C;
+    }
+    
+        
+// Drucksensor initialisieren  
+// -----------------------------------------------
+void BMP085_old::Init () 
+    {
+    AC1 = twi_readshort(BMP085_ADRESS, 0xaa);
+    AC2 = twi_readshort(BMP085_ADRESS, 0xac);
+    AC3 = twi_readshort(BMP085_ADRESS, 0xae);
+    AC4 = twi_readshort(BMP085_ADRESS, 0xb0);
+    AC5 = twi_readshort(BMP085_ADRESS, 0xb2);
+    AC6 = twi_readshort(BMP085_ADRESS, 0xb4);
+    B1 = twi_readshort(BMP085_ADRESS, 0xb6);
+    B2 = twi_readshort(BMP085_ADRESS, 0xb8);
+    MB = twi_readshort(BMP085_ADRESS, 0xba);
+    MC = twi_readshort(BMP085_ADRESS, 0xbc);
+    MD = twi_readshort(BMP085_ADRESS, 0xbe);
+    }
+    
+    
+// -----------------------------------------------
+unsigned short BMP085_old::twi_readshort (int id, int addr) {
+    unsigned short i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+unsigned long BMP085_old::twi_readlong (int id, int addr) {
+    unsigned long i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 16;
+    i |= i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+void BMP085_old::twi_writechar (int id, int addr, int dat) {
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+    i2c_.write(dat);
+    i2c_.stop();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085_old.h	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,34 @@
+ 
+#ifndef BMP085_I2C_H
+#define BMP085_I2C_H
+
+#include "mbed.h"
+
+class BMP085_old
+    {
+    private:
+        I2C i2c_;
+        
+    public:
+        BMP085_old(PinName sda, PinName scl);
+    
+        void    Update();
+        float   Temperature;
+        float   Pressure;
+        float   CalcAltitude(float Press);
+        short   oss;
+    
+    protected:
+        void Init();
+        unsigned short twi_readshort (int, int);
+        unsigned long twi_readlong (int, int);
+        void twi_writechar (int, int, int);
+    
+    
+    private:
+    
+        short AC1, AC2, AC3, B1, B2, MB, MC, MD;
+        unsigned short AC4, AC5, AC6;
+    };
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/BMP085/BMP085.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+#include "BMP085.h"
+
+BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS)
+{
+    // initialize BMP085 with settings
+    //writeRegister(0xf4, 0x2e); // TODO: was macht das + register in header!
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/BMP085/BMP085.h	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,34 @@
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "I2C_Sensor.h"
+
+// I2C address
+#define BMP085_I2C_ADDRESS 0xEE
+
+class BMP085 : public I2C_Sensor
+{           
+    public:
+        BMP085(PinName sda, PinName scl);
+        
+        float data[3];
+        
+        void read();
+        
+        void calibrate(int s);
+        
+        float get_angle();
+         
+    private:
+        // raw data and function to measure it
+        int raw[3];
+        void readraw();
+        
+        // calibration parameters and their saving
+        int Min[3];
+        int Max[3];
+        float scale[3];
+        float offset[3];
+};
+
+#endif

--- a/Sensors/Comp/HMC5883.cpp	Thu Oct 25 19:27:56 2012 +0000
+++ b/Sensors/Comp/HMC5883.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
 #include "HMC5883.h"
 
-HMC5883::HMC5883(PinName sda, PinName scl) :  local("local"), i2c(sda, scl)
+HMC5883::HMC5883(PinName sda, PinName scl) :  i2c(sda, scl), local("local")
 {
-    i2c.frequency(400000); // zu testen!!
+    i2c.frequency(400000); // TODO: zu testen!!
     
     // load calibration values
     FILE *fp = fopen("/local/compass.txt", "r");
@@ -18,8 +18,8 @@
     writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
     writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
     
-    /*
-    // Scaling with testmode (not important, just from data sheet)
+    /*          (not important, just from data sheet)
+    // Scaling with testmode
     for(int j = 0; j < 3; j++) // set all scales to 1 first so the measurement for scaling is not already scaled
         scale[j] = 1;
     
@@ -105,18 +105,16 @@
     float Heading;
     
     Heading = Rad2Deg * atan2(data[0],data[1]);
-    
     Heading += 1.367;                   //bei Ost-Deklination  += DecAngle, bei West-Deklination -= DecAngle
                                         //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
                                         //Bern ca. 1.367 Grad Ost
                                         //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
- 
     if(Heading < 0)  
         Heading += 360;                 // minimum 0 degree
         
     if(Heading > 360)   
         Heading -= 360;                 // maximum 360 degree
-        
+
     return Heading;
 }
     

--- a/Sensors/Comp/HMC5883.h	Thu Oct 25 19:27:56 2012 +0000
+++ b/Sensors/Comp/HMC5883.h	Sat Oct 27 10:53:43 2012 +0000
@@ -16,18 +16,9 @@
     public:
         HMC5883(PinName sda, PinName scl);
         
-        //my
         float data[3];
-        
         void read();
-        
         void calibrate(int s);
-        int Min[3];
-        int Max[3];
-        float scale[3];
-        float offset[3];
-        LocalFileSystem local;
-        
         float get_angle();
          
     private:
@@ -41,6 +32,12 @@
         void writeReg(char address, char data);
         void readMultiReg(char address, char* output, int size);
         
+        // calibration parameters and their saving
+        int Min[3];
+        int Max[3];
+        float scale[3];
+        float offset[3];
+        LocalFileSystem local;
 };
 
 #endif

--- a/Sensors/Gyro/L3G4200D.cpp	Thu Oct 25 19:27:56 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -7,7 +7,7 @@
 
 L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl)
 {
-    i2c.frequency(400000);
+    i2c.frequency(100000);
     // Turns on the L3G4200D's gyro and places it in normal mode.
     // Normal power mode, all axes enabled
     
@@ -77,11 +77,29 @@
     // assert the MSB of the address to get the gyro 
     // to do slave-transmit subaddress updating.
     buffer[0] = L3G4200D_OUT_X_L | (1 << 7);
-    i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); 
+    
+    //i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); 
+    
+    //i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); 
+    
+    L3G4200D_OUT_X_L
+    L3G4200D_OUT_X_H
+    L3G4200D_OUT_Y_L
+    L3G4200D_OUT_Y_H
+    L3G4200D_OUT_Z_L
+    L3G4200D_OUT_Z_H
     
-    i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); 
+    i2c.start();
+    i2c.write(L3G4200D_I2C_ADDRESS);
+    i2c.write(L3G4200D_OUT_X_L);
 
-    data[0] = (short) (buffer[1] << 8 | buffer[0]);
+    i2c.start();
+    i2c.write(L3G4200D_I2C_ADDRESS | 1);
+    buffer[1]  = i2c.read(1) << 8;
+    buffer[1] |= i2c.read(0);
+    i2c.stop();
+    
+    //data[0] = (short) (buffer[1] << 8 | buffer[0]);
     data[1] = (short) (buffer[3] << 8 | buffer[2]);
     data[2] = (short) (buffer[5] << 8 | buffer[4]);
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,41 @@
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+// calculate the 8-Bit write/read I2C-Address from the 7-Bit adress of the device
+#define GET_I2C_WRITE_ADDRESS(ADR)  (ADR << 1&0xFE) // ADR & 1111 1110
+#define GET_I2C_READ_ADDRESS(ADR)   (ADR << 1|0x01) // ADR | 0000 0001
+
+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!
+}
+
+void saveCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "w");
+    for(int i = 0; i < size; i++)
+        fprintf(fp, "%f\r\n", values[i]);
+    fclose(fp);
+}
+
+void loadCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "r");
+    for(int i = 0; i < size; i++)
+        fscanf(fp, "%f", &values[i]);
+    fclose(fp);
+}
+
+void I2C_Sensor::writeRegister(char address, char data){ 
+    char tx[2];
+    tx[0] = address;
+    tx[1] = data;
+    i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), tx, 2);
+}
+
+void I2C_Sensor::readMultiRegister(char address, char* output, int size) {
+    i2c.write (GET_I2C_WRITE_ADDRESS(i2c_address), &address, 1);      //tell it from which register to read
+    i2c.read  (GET_I2C_READ_ADDRESS(i2c_address), output, size);      //tell it where to store the data read
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.h	Sat Oct 27 10:53:43 2012 +0000
@@ -0,0 +1,30 @@
+#ifndef I2C_Sensor_H
+#define I2C_Sensor_H
+
+class I2C_Sensor
+{           
+    public:
+        I2C_Sensor(PinName sda, PinName scl, int8_t address);
+        
+        float data[3];
+        void read();
+        void calibrate();
+        
+    protected:
+        // I2C functions
+        void writeRegister(char address, char data);
+        void readMultiRegister(char address, char* output, int size);
+        
+    private:
+        I2C i2c;            // I2C-Bus
+    
+        int8_t i2c_address; // address
+        
+        // raw data and function to measure it
+        int raw[3];
+        void readraw();
+        
+        LocalFileSystem local; // file access to save calibration values
+};
+
+#endif

--- a/main.cpp	Thu Oct 25 19:27:56 2012 +0000
+++ b/main.cpp	Sat Oct 27 10:53:43 2012 +0000
@@ -4,64 +4,64 @@
 #include "L3G4200D.h"   // Gyro (Gyroscope)
 #include "ADXL345.h"    // Acc (Accelerometer)
 #include "HMC5883.h"    // Comp (Compass)
-#include "BMP085.h"     // Alt (Altitude sensor)
+#include "BMP085_old.h"     // Alt (Altitude sensor)
 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
 #include "Servo.h"      // Motor PPM
 #include "PID.h"        // PID Library by Aaron Berk
 #include "IntCtrl.h"    // Interrupt Control by Roland Elmiger
 
-#define PI              3.1415926535897932384626433832795
-#define Rad2Deg         57.295779513082320876798154814105
-#define RATE            0.02 // speed of Ticker/PID
-
-//#define COMPASSCALIBRATE decomment if you want to calibrate the Compass on start
+#define PI              3.1415926535897932384626433832795   // ratio of a circle's circumference to its diameter
+#define Rad2Deg         57.295779513082320876798154814105   // ratio between radians and degree (360/2Pi)
+#define RATE            0.0007                               // speed of the interrupt for Sensors and PID
 
-Timer GlobalTimer; // global time to calculate processing speed
-Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
-PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
-PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle
-//PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25 
+//#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
 
-// initialisation of hardware
+Timer GlobalTimer;                      // global time to calculate processing speed
+Ticker Datagetter;                      // timecontrolled interrupt to get data form IMU and RC
+
+// initialisation of hardware (see includes for more info)
 LED         LEDs;
 PC          pc(USBTX, USBRX, 57600);
 L3G4200D    Gyro(p28, p27);
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27);
-BMP085      Alt(p28, p27);
+BMP085_old      Alt(p28, p27);
 RC_Channel  RC[] = {(p11), (p12), (p13), (p14)}; // noooo p19/p20!!!!
 Servo       Motor[] = {(p15), (p16), (p17), (p18)};
 
-// variables for loop
-unsigned long   dt_get_data = 0;
+PID controller(1.0, 0.0, 0.0, RATE);  // test PID controller for throttle
+PID pid(1.0, 1.0, 0.0, RATE);         // test PID controller for throttle
+//PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25 
+
+
+// global varibles
+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}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
-float           tempangle = 0;
-int             Motorvalue[3];
-
+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 pidtester;
 
-void get_Data()
+void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
 {
     time_read_sensors = GlobalTimer.read_us();
     
-    GPIO_IntDisable(0, 18, 2); // abschalten der Pins 11-14 mit Göttis library
+    // read data from sensors
+    /*GPIO_IntDisable(0, 18, 2); // abschalten der Pins 11-14 mit Göttis library
     GPIO_IntDisable(0, 17, 2);
     GPIO_IntDisable(0, 16, 2);
-    GPIO_IntDisable(0, 15, 2);
+    GPIO_IntDisable(0, 15, 2);*/
     //__disable_irq(); // test, deactivate all interrupts, I2C working?
-    // read data from sensors
-    Gyro.read(); // einzeln testen! mit LEDs
-    Acc.read();
-    Comp.read();
+    Gyro.read();
+    //Acc.read();
+    //Comp.read();
     //Alt.Update(); TODO braucht zu lange zum auslesen!
     //__enable_irq();
-    GPIO_IntEnable(0, 18, 2); // schaltet enable wirklich wieder ein?? änderungsbedarf??
+    /*GPIO_IntEnable(0, 18, 2); // schaltet die PINs wieder ein
     GPIO_IntEnable(0, 17, 2);
     GPIO_IntEnable(0, 16, 2);
-    GPIO_IntEnable(0, 15, 2);
+    GPIO_IntEnable(0, 15, 2);*/
     
     dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
     
@@ -86,9 +86,11 @@
     */
 }
 
-
-int main() {
-    //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
+int main() { // main programm only used for initialisation and debug output
+    NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
+    /*NVIC_SetPriority(I2C0_IRQn, 1); //I2C Priorität?
+    NVIC_SetPriority(I2C1_IRQn, 1);
+    NVIC_SetPriority(I2C2_IRQn, 1);*/
     
     #ifdef COMPASSCALIBRATE
         pc.locate(10,5);
@@ -101,7 +103,7 @@
     pc.printf("Flybed v0.2");
     LEDs.roll(2);
     
-    // Start!
+    // Start! TODO: Motor und RC start (armed....?)
     GlobalTimer.start();
     Datagetter.attach(&get_Data, RATE);     // start to get data all RATEms
     
@@ -117,27 +119,27 @@
         pc.locate(10,12);
         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.printf("Comp_scale: %6.4f %6.4f %6.4f   ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
+        //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("pidtester: %6.1f   RC: %d %d %d %d     ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
         
         pc.locate(10,19);
-        pc.printf("RC0: %d :[", RC[0].read());
+        pc.printf("RC0: %4d :[", RC[0].read());
         for (int i = 0; i < (RC[0].read() - 1000)/17; i++)
             pc.printf("=");
         pc.printf("                                                       ");
         pc.locate(10,20);
-        pc.printf("RC1: %d :[", RC[1].read());
+        pc.printf("RC1: %4d :[", RC[1].read());
         for (int i = 0; i < (RC[1].read() - 1000)/17; i++)
             pc.printf("=");
         pc.printf("                                                       ");
         pc.locate(10,21);
-        pc.printf("RC2: %d :[", RC[2].read());
+        pc.printf("RC2: %4d :[", RC[2].read());
         for (int i = 0; i < (RC[2].read() - 1000)/17; i++)
             pc.printf("=");
         pc.printf("                                                       ");
         pc.locate(10,22);
-        pc.printf("RC3: %d :[", RC[3].read());
+        pc.printf("RC3: %4d :[", RC[3].read());
         for (int i = 0; i < (RC[3].read() - 1000)/17; i++)
             pc.printf("=");
         pc.printf("                                                       ");