IMU_10DOF.cpp

00001 #include "IMU_10DOF.h"
00002 
00003 IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : Sensor(p28, p27), Gyro(sda, scl), Acc(sda, scl), Comp(sda, scl), Alt(sda,scl)
00004 {
00005     dt = 0;
00006     dt_sensors = 0;
00007     time_for_dt = 0;
00008     time_for_dt_sensors = 0;
00009     
00010     angle = Filter.angle;           // initialize array pointer
00011     
00012     Sensor.calibrate(50, 0.02);
00013     
00014     LocalTimer.start();
00015 }
00016 
00017 void IMU_10DOF::readAngles()
00018 {
00019     time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors
00020     //mpu.readGyro();
00021     //mpu.readAcc();
00022     Sensor.read();
00023     //Gyro.read(); // reading sensor data
00024     //Acc.read();
00025     //Comp.read();    
00026     dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors
00027 
00028     // meassure dt for the filter
00029     dt = LocalTimer.read() - time_for_dt; // time in s since last loop
00030     time_for_dt = LocalTimer.read();      // set new time for next measurement
00031     
00032     //Filter.compute(dt, mpu.Gyro, mpu.Acc, Comp.data);
00033     Filter.compute(dt, Sensor.data_gyro, Sensor.data_acc, Comp.data);
00034     //Filter.compute(dt, Gyro.data, Acc.data, Comp.data);
00035 }
00036 
00037 void IMU_10DOF::readAltitude()
00038 {
00039     Alt.read();
00040     temperature = Alt.Temperature; // copy all resulting measurements
00041     pressure    = Alt.Pressure;
00042     altitude    = Alt.Altitude;
00043 }