Diff: IMU/IMU_10DOF.cpp

Revision:

0:37f0c1e8fa66

Child:

1:60882db03b0f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/IMU_10DOF.cpp	Tue Sep 08 13:38:10 2015 +0000
@@ -0,0 +1,27 @@
+#include "IMU_10DOF.h"
+
+IMU_10DOF::IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : mpu(MOSI, MISO, SCLK, CS)
+{
+    dt = 0;
+    dt_sensors = 0;
+    time_for_dt = 0;
+    time_for_dt_sensors = 0;
+    
+    angle = Filter.angle;           // initialize array pointer
+    
+    LocalTimer.start();
+}
+
+void IMU_10DOF::readAngles()
+{
+    time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors
+    //mpu.readGyro(); // reading sensor data
+    //mpu.readAcc();
+    dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors
+
+    // meassure dt since last measurement for the filter
+    dt = LocalTimer.read() - time_for_dt; // time in s since last loop
+    time_for_dt = LocalTimer.read();      // set new time for next measurement
+    
+    Filter.compute(dt, mpu.Gyro, mpu.Acc, mpu.Acc);
+}
\ No newline at end of file