IMU_10DOF.cpp

00001 #include "IMU_10DOF.h"
00002 
00003 IMU_10DOF::IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName SDA, PinName SCL) : mpu(MOSI, MISO, SCLK, CS), mpu2(SDA, SCL)
00004 {
00005     dt = 0;
00006     dt_sensors = 0;
00007     
00008     angle = Filter.angle;           // initialize array pointer
00009     
00010     LoopTimer.start();
00011 }
00012 
00013 void IMU_10DOF::readAngles()
00014 {
00015     SensorTimer.start(); // start time for measuring sensors
00016     mpu.readGyro(); // reading sensor data
00017     mpu.readAcc();
00018     mpu2.read(); // reading sensor data
00019     SensorTimer.stop(); // stop time for measuring sensors
00020     dt_sensors = SensorTimer.read();
00021     SensorTimer.reset();
00022 
00023     // meassure dt since last measurement for the filter
00024     dt = LoopTimer.read(); // time in s since last loop
00025     LoopTimer.reset();
00026     
00027     Filter.compute(dt, mpu.Gyro, mpu.Acc, mpu.Acc);
00028     //Filter.compute(dt, mpu2.data_gyro, mpu2.data_acc, mpu2.data_acc);
00029 }