Revision 4:f62337b907e5, committed 2013-08-29

Comitter:

maetugr

Date:

Thu Aug 29 13:52:30 2013 +0000

Parent:

3:6dbefedce7fe

Commit message:

The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085_old.cpp	Thu Aug 29 13:52:30 2013 +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	Thu Aug 29 13:52:30 2013 +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

--- a/IMU/IMU_10DOF.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/IMU_10DOF.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -1,17 +1,18 @@
 #include "IMU_10DOF.h"
 
-IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : Gyro(sda, scl), Acc(sda, scl), Comp(sda, scl)
+IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : Gyro(sda, scl), Acc(sda, scl), Comp(sda, scl), Alt(sda,scl)
 {
     dt = 0;
     dt_sensors = 0;
     time_for_dt = 0;
     time_for_dt_sensors = 0;
-    angle = Filter.angle;
+    
+    angle = Filter.angle;           // initialize array pointer
     
     LocalTimer.start();
 }
 
-void IMU_10DOF::read()
+void IMU_10DOF::readAngles()
 {
     time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors
     Gyro.read(); // reading sensor data
@@ -24,4 +25,12 @@
     time_for_dt = LocalTimer.read();      // set new time for next measurement
     
     Filter.compute(dt, Gyro.data, Acc.data, Comp.data);
+}
+
+void IMU_10DOF::readAltitude()
+{
+    Alt.read();
+    temperature = Alt.Temperature; // copy all resulting measurements
+    pressure    = Alt.Pressure;
+    altitude    = Alt.Altitude;
 }
\ No newline at end of file

--- a/IMU/IMU_10DOF.h	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/IMU_10DOF.h	Thu Aug 29 13:52:30 2013 +0000
@@ -7,15 +7,20 @@
 #include "L3G4200D.h"   // Gyro (Gyroscope)
 #include "ADXL345.h"    // Acc (Accelerometer)
 #include "HMC5883.h"    // Comp (Compass)
-#include "IMU_Filter.h" // Class to calculate position angles
+#include "BMP085.h"     // Alt (Altitude sensor or Barometer)
+#include "IMU_Filter.h" // Class to calculate position angles  (algorithm from S.O.H. Madgwick, see header file for info)
 
 class IMU_10DOF
 {           
     public:
         IMU_10DOF(PinName sda, PinName scl);
-        void read();                    // read all axis from register to array data
+        void readAngles();              // read all axis from register to array data
+        void readAltitude();            // read all axis from register to array data
         
-        float * angle;                  // where the measured and calculated angles are saved
+        float * angle;                  // where the measured and calculated data is saved
+        float temperature;
+        float pressure;
+        float altitude;
         
         float dt;                       // local time to calculate processing speed for entire loop and just reading sensors
         float dt_sensors;               // |
@@ -27,6 +32,8 @@
         L3G4200D    Gyro;               // All sensors Hardwaredrivers
         ADXL345     Acc;
         HMC5883     Comp;
+        BMP085      Alt;
+        
         IMU_Filter  Filter;             // Filterclass to join sensor data
 };
 

--- a/IMU/IMU_Filter/IMU_Filter.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/IMU_Filter/IMU_Filter.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -51,7 +51,7 @@
 }
 
 //------------------------------------------------------------------------------------------------------------------
-// Code copied from S.O.H. Madgwick (https://code.google.com/p/imumargalgorithm30042010sohm/)
+// Code copied from S.O.H. Madgwick (File AHRS.c from https://code.google.com/p/imumargalgorithm30042010sohm/)
 //------------------------------------------------------------------------------------------------------------------
 
 // IMU

--- a/IMU/IMU_Filter/IMU_Filter.h	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/IMU_Filter/IMU_Filter.h	Thu Aug 29 13:52:30 2013 +0000
@@ -1,3 +1,26 @@
+//=====================================================================================================
+// ALGORITHM COPIED FROM http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+// Algorithm Author: S.O.H. Madgwick
+// Algorithm Date: 25th August 2010
+//=====================================================================================================
+// Description:
+//
+// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
+// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
+// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
+// axis only.
+//
+// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
+//
+// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
+// orientation.  See my report for an overview of the use of quaternions in this application.
+//
+// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
+// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
+// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
+//
+//=====================================================================================================
+
 // by MaEtUgR
 
 #ifndef IMU_FILTER_H

--- a/IMU/Sensors/Acc/ADXL345.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Acc/ADXL345.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -2,6 +2,7 @@
 
 ADXL345::ADXL345(PinName sda, PinName scl) : I2C_Sensor(sda, scl, ADXL345_I2C_ADDRESS)
 {
+    #warning these three offsets are calibration values to make shure the measurement is 0 when no acceleration is present
     offset[0] = -9.1; // offset calculated by hand... (min + ((max - min) / 2)
     offset[1] = -7.1; // TODO: make this automatic with saving to filesystem
     offset[2] = 6;

--- a/IMU/Sensors/Alt/BMP085.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Alt/BMP085.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -1,57 +1,79 @@
 #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!
+BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS) {
+    // read calibration data from E2PROM, needed for formulas in read function
+    char buffer[22];                                  // 8-Bit pieces of axis data
+    readMultiRegister(BMP085_CAL_AC1, buffer, 22);       // read all calibration registers in one time 22 Byte = 176 Bit
     
+    AC1 = (short) (buffer[0] << 8 | buffer[1]);         // join 8-Bit pieces to 16-bit short integers
+    AC2 = (short) (buffer[2] << 8 | buffer[3]);
+    AC3 = (short) (buffer[4] << 8 | buffer[5]);
+    AC4 = (unsigned short) (buffer[6] << 8 | buffer[7]);      // unsigned !!
+    AC5 = (unsigned short) (buffer[8] << 8 | buffer[9]);
+    AC6 = (unsigned short) (buffer[10] << 8 | buffer[11]);
+    B1 = (short) (buffer[12] << 8 | buffer[13]);
+    B2 = (short) (buffer[14] << 8 | buffer[15]);
+    MB = (short) (buffer[16] << 8 | buffer[17]);
+    MC = (short) (buffer[18] << 8 | buffer[19]);
+    MD = (short) (buffer[20] << 8 | buffer[21]);
+    
+    
+    oss = 0; // set Oversampling of Sensor
+}
+
+void BMP085::readraw() {
 }
 
-/*void BMP085::read() 
-    {
-    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+void BMP085::read() {
+    unsigned short Uncompensated_Temperature;
+    long P, Uncompensated_Pressure, X1, X2, X3, B3, B5, B6;
     unsigned long B4, B7;
-
-    // TODO: writeRegister(0xf4, 0x2e); ?!!!
-    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
-    // Wait at least 4.5ms
-    wait(0.005);
-    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+    
+    // read uncompensated Temperature
+    writeRegister(BMP085_CONTROL, READ_TEMPERATURE); // say the sensor we want to read the temperature
+    wait(0.005); // Wait at least 4.5ms (written in data sheet)
+    char buffer[3];  // TODO: nur 2 wenn unten nicht gebraucht                                           // read 16-Bit Temperature (2 registers)
+    readMultiRegister(BMP085_CONTROL_OUTPUT, buffer, 2);        
+    Uncompensated_Temperature =  buffer[0] << 8 | buffer[1];               // join 8-Bit pieces to 16-bit short integer
     
-    X1 = ((UTemp - AC6) * AC5) >> 15;
+    // calculate real Temperature
+    X1 = ((Uncompensated_Temperature - 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;
+    #warning 33 is a calibration value for 3.3 degree shifft of temperature
+    B5 = X1 + X2 - ((33 << 4) - 8); // TODO: I added this term because i think my sensor is about 3.3 degree off which makes a difference in absolute altitude
+    Temperature = (float)((B5 + 8) >> 4)/10.0; // we want temperature in degree with digit after comma
+    
+    // read uncompensated Pressure
+    writeRegister(BMP085_CONTROL, READ_PRESSURE + (oss << 6)); // say the sensor we want to read the pressure
+    wait(0.005); // Wait at least 4.5ms (written in data sheet) TODO: oss fest, times vary and calculation of B3                         
+    readMultiRegister(BMP085_CONTROL_OUTPUT, buffer, 3);                                                    // read 24-Bit Pressure (3 registers)
+    Uncompensated_Pressure = (unsigned int) (buffer[0] << 16 | buffer[1] << 8 | buffer[0]) >> (8 - oss);    // join 8-Bit pieces to 24-bit integer
+    
+    // calculate real Pressure
+    B6 = B5 - 4000;     // B5 is updated by real temperature above
+    X1 = (B2 * ( (B6 * B6) >> 12 )) >> 11;
     X2 = (AC2 * B6) >> 11;
     X3 = X1 + X2;
-    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
+    B3 = (((AC1 * 4 + X3) << oss) + 2) >> 2;
 
     X1 = (AC3 * B6) >> 13;
-    X2 = (B1 * (B6 * B6) >> 12) >> 16;
+    X2 = (B1 * ( (B6 * B6) >> 12) ) >> 16;
     X3 = ((X1 + X2) + 2) >> 2;
-    B4 = AC4 * (X3 + 32768) >> 15;
+    B4 = AC4 * ((unsigned long)X3 + 32768) >> 15;
     
-    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+    B7 = ((unsigned long)Uncompensated_Pressure - B3) * (50000 >> oss);
     
-    if (B7 < 0x80000000) 
-        {
-        P = (2 * B7) / B4;
-        }
+    if (B7 < 0x80000000)
+        P = (B7 * 2) / B4;
     else 
-        {
-        P = 2* (B7 / B4);
-        }
+        P = (B7 / B4) * 2;
+    
     X1 = (P >> 8) * (P >> 8);
     X1 = (X1 * 3038) >> 16;
     X2 = (-7357 * P) >> 16;    
     P = P + ((X1 + X2 + 3791) >> 4);
     Pressure = (float)P / 100.0;
-    }*/
\ No newline at end of file
+    
+    // calculate height out of the pressure
+    Altitude = 44330 * (1.0 - pow((Pressure / 1013.25), 1/5.25588));
+}
\ No newline at end of file

--- a/IMU/Sensors/Alt/BMP085.h	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Alt/BMP085.h	Thu Aug 29 13:52:30 2013 +0000
@@ -8,27 +8,53 @@
 
 #define BMP085_I2C_ADDRESS 0xEE
 
-class BMP085 : public I2C_Sensor
-{           
+// register addresses
+#define BMP085_CAL_AC1           0xAA  // R   Calibration data (16 bits)
+#define BMP085_CAL_AC2           0xAC  // R   Calibration data (16 bits)
+#define BMP085_CAL_AC3           0xAE  // R   Calibration data (16 bits)    
+#define BMP085_CAL_AC4           0xB0  // R   Calibration data (16 bits)
+#define BMP085_CAL_AC5           0xB2  // R   Calibration data (16 bits)
+#define BMP085_CAL_AC6           0xB4  // R   Calibration data (16 bits)
+#define BMP085_CAL_B1            0xB6  // R   Calibration data (16 bits)
+#define BMP085_CAL_B2            0xB8  // R   Calibration data (16 bits)
+#define BMP085_CAL_MB            0xBA  // R   Calibration data (16 bits)
+#define BMP085_CAL_MC            0xBC  // R   Calibration data (16 bits)
+#define BMP085_CAL_MD            0xBE  // R   Calibration data (16 bits)
+#define BMP085_CONTROL           0xF4  // W   Control register 
+#define BMP085_CONTROL_OUTPUT    0xF6  // R   Output registers 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB
+
+#define BMP085_SOFTRESET         0xE0  // -- unused registers
+#define BMP085_VERSION           0xD1  // ML_VERSION  pos=0 len=4 msk=0F  AL_VERSION pos=4 len=4 msk=f0
+#define BMP085_CHIPID            0xD0  // pos=0 mask=FF len=8 BMP085_CHIP_ID=0x55
+
+// BMP085 Modes
+#define MODE_ULTRA_LOW_POWER    0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5
+#define MODE_STANDARD           1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4
+#define MODE_HIGHRES            2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3
+#define MODE_ULTRA_HIGHRES      3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25
+                  // "Sampling rate can be increased to 128 samples per second (standard mode) for
+                  // dynamic measurement.In this case it is sufficient to measure temperature only 
+                  // once per second and to use this value for all pressure measurements during period."
+                  // (from BMP085 datasheet Rev1.2 page 10).
+// Control register
+#define READ_TEMPERATURE        0x2E 
+#define READ_PRESSURE           0x34 
+//Other
+#define MSLP                    101325          // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
+
+class BMP085 : public I2C_Sensor {           
     public:
         BMP085(PinName sda, PinName scl);
         
-        //virtual void read();
-        
-        void calibrate(int s);
-        
-        float get_height();
+        virtual void read();
+        float Temperature, Pressure, Altitude;
          
     private:
-        // raw data and function to measure it
-        int raw[3];
-        //void readraw();
+        short AC1, AC2, AC3, B1, B2, MB, MC, MD; // calibration data
+        unsigned short AC4, AC5, AC6;
+        short oss;
         
-        // calibration parameters and their saving
-        int Min[3];
-        int Max[3];
-        float scale[3];
-        float offset[3];
+        virtual void readraw();
 };
 
 #endif

--- a/IMU/Sensors/Comp/HMC5883.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Comp/HMC5883.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -2,6 +2,7 @@
 
 HMC5883::HMC5883(PinName sda, PinName scl) : I2C_Sensor(sda, scl, HMC5883_I2C_ADDRESS)
 {   
+    #warning these three offsets are calibration values to get |MAX| = |MIN|
     offset[0] = -155; // offset calculated by hand... (min + ((max - min) / 2)
     offset[1] = -142; // TODO: make this automatic with saving to filesystem
     offset[2] = -33.5;

--- a/IMU/Sensors/Gyro/L3G4200D.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Gyro/L3G4200D.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -1,7 +1,6 @@
 #include "L3G4200D.h"
 
-L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS)
-{
+L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS) {
     // Turns on the L3G4200D's gyro and places it in normal mode.
     // Normal power mode, all axes enabled (for detailed info see datasheet)
     
@@ -30,21 +29,18 @@
     //calibrate(50, 0.01);
 }
 
-void L3G4200D::read()
-{
+void L3G4200D::read() {
     readraw();                                          // read raw measurement data
     
     for (int i = 0; i < 3; i++)
             data[i] = (raw[i] - offset[i])*0.07;               // subtract offset from calibration and multiply unit factor (datasheet s.10)
 }
 
-int L3G4200D::readTemp()
-{
-    return (short) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
+int L3G4200D::readTemp() {
+    return (char) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
 }
 
-void L3G4200D::readraw()
-{
+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: why?!   | (1 << 7)
@@ -54,8 +50,7 @@
     raw[2] = (short) (buffer[5] << 8 | buffer[4]);
 }
 
-void L3G4200D::calibrate(int times, float separation_time)
-{
+void L3G4200D::calibrate(int times, float separation_time) {
     // calibrate sensor with an average of count samples (result of calibration stored in offset[])
     float calib[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
     

--- a/IMU/Sensors/Gyro/L3G4200D.h	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/Gyro/L3G4200D.h	Thu Aug 29 13:52:30 2013 +0000
@@ -40,8 +40,7 @@
 #define L3G4200D_INT1_THS_ZL    0x37
 #define L3G4200D_INT1_DURATION  0x38
 
-class L3G4200D : public I2C_Sensor
-{
+class L3G4200D : public I2C_Sensor {
     public:            
         L3G4200D(PinName sda, PinName scl);                 // constructor, uses I2C_Sensor class
         virtual void read();                                // read all axis from register to array data

--- a/IMU/Sensors/I2C_Sensor.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/I2C_Sensor.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -27,7 +27,7 @@
     fclose(fp);
 }
 
-//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// TODO TODO TODO--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 // ATTENTION!!! there was a problem with other interrupts disturbing the i2c communication of the chip... that's why I use I2C to initialise the sonsors and MODI2C to get the data (only made with readMultiRegister)
 // IT DIDN'T WORK STABLE IN OTHER COMBINATIONS (if someone has an idea why please let me know)
 //--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

--- a/IMU/Sensors/I2C_Sensor.h	Tue Aug 27 22:18:25 2013 +0000
+++ b/IMU/Sensors/I2C_Sensor.h	Thu Aug 29 13:52:30 2013 +0000
@@ -25,11 +25,11 @@
         void readMultiRegister(char reg, char* output, int size);
         
         // raw data and function to measure it
-        int raw[3];
+        short raw[3];
         virtual void readraw() = 0;
         
     private:
-        I2C i2c;       // original mbed I2C-library just to initialise the control registers
+        I2C i2c;            // original mbed I2C-library just to initialise the control registers
         char i2c_address;   // address
         
         LocalFileSystem local; // file access to save calibration values

--- a/main.cpp	Tue Aug 27 22:18:25 2013 +0000
+++ b/main.cpp	Thu Aug 29 13:52:30 2013 +0000
@@ -12,7 +12,7 @@
 Ticker Dutycycler;                      // timecontrolled interrupt for exact timed control loop
 
 void dutycycle() { // method which is called by the Ticker Dutycycler every RATE seconds
-    IMU.read();
+    IMU.readAngles();
 }
 
 void executer() {
@@ -24,9 +24,12 @@
     Dutycycler.attach(&dutycycle, RATE);     // start to process all RATE seconds
     pc.attach(&executer);
     while(1) {
+        #warning The current version has some hardcoded calibration values, change them in order to get high precision, to find them look out for_ warnings (sorry for_ that hope to get time to develop autocalibration)
         // just putting out the angle on console
-        pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.dt, IMU.dt_sensors); // Output for Python
-        wait(0.01);
+        IMU.readAltitude(); // reading altitude takes much more time than the angles -> don't do this in your fast loop
+        pc.printf("%.1f,%.1f,%.1f,%.1f'C,%.1fhPa,%.1fmaS,%.5fs,%.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.temperature, IMU.pressure, IMU.altitude, IMU.dt, IMU.dt_sensors); // Output for Python
+        
+        wait(0.01); // this is to avoid buffer overflow in the Computers UART-Controller
 
         LEDs.tilt(1);
     }