An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
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); }