Diff: IMU.h

Revision:

2:7439607ccd51

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h	Tue Oct 15 18:30:46 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "MPU6050.h"
+#include "IMUfilter.h"
+
+#ifndef IMU_H
+#define IMU_H
+
+Mutex        IMU_mutex;
+const float  IMU_dt = 0.010;
+IMUfilter    IMU_filter(IMU_dt,40);
+
+void IMU_thread_routine (void const *args){
+  const uint32_t wait_time_ms = (uint32_t)((float)1000.0 * IMU_dt);
+  DigitalOut led2(LED2);
+  led2 = 0;
+  MPU6050 IMU_sensor(p28, p27); //NOTE: set here I2C pin!!!
+  IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
+  IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
+  
+  led2 = 1;
+  while(true){
+    float a[3], g[3];
+    
+    IMU_sensor.getAccelero(a);
+    IMU_sensor.getGyro(g);
+    
+    IMU_mutex.lock();
+    IMU_filter.updateFilter(g[0], g[1], g[2],  a[0], a[1], a[2]);
+    IMU_mutex.unlock();
+    
+    led2 = !led2;
+    
+    Thread::wait(wait_time_ms);
+  }
+}
+
+
+inline void IMU_filter_getYawPitchRoll (float &y, float &p, float &r){  
+  IMU_mutex.lock();
+  IMU_filter.computeEuler();
+  y = IMU_filter.getYaw();
+  p = IMU_filter.getPitch();
+  r = IMU_filter.getRoll();
+  IMU_mutex.unlock();
+}
+
+
+#endif
\ No newline at end of file