IMU_10DOF.cpp

00001 #include "IMU_10DOF.h"
00002 
00003 IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : 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     LocalTimer.start();
00013 }
00014 
00015 void IMU_10DOF::readAngles()
00016 {
00017     time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors
00018     Gyro.read(); // reading sensor data
00019     Acc.read();
00020     Comp.read();    
00021     dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors
00022 
00023     // meassure dt for the filter
00024     dt = LocalTimer.read() - time_for_dt; // time in s since last loop
00025     time_for_dt = LocalTimer.read();      // set new time for next measurement
00026     
00027     Filter.compute(dt, Gyro.data, Acc.data, Comp.data);
00028 }
00029 
00030 void IMU_10DOF::readAltitude()
00031 {
00032     Alt.read();
00033     temperature = Alt.Temperature; // copy all resulting measurements
00034     pressure    = Alt.Pressure;
00035     altitude    = Alt.Altitude;
00036 }