Diff: IMU.h

Revision:

5:33abcc31b0aa

Parent:

4:46f3c3dd5977

--- a/IMU.h	Sat May 03 13:22:30 2014 +0000
+++ b/IMU.h	Mon May 12 22:58:06 2014 +0000
@@ -0,0 +1,72 @@
+/*typedef struct {
+    float * ypr;
+    float alt;
+    float temp;
+    int time;
+} IMU_Ejection;
+
+typedef struct {
+    double target_imu_dt;
+    double actual_imu_dt;
+    double average_imu_dt;
+    double average_imu_dt_k;
+    
+    void reset(){
+        target_imu_dt = TARGET_IMU_DT;
+        actual_imu_dt = TARGET_IMU_DT;
+        average_imu_dt = TARGET_IMU_DT;
+        average_imu_dt_k = AVERAGE_DT_K_GAIN;
+    }
+} IMU_QuadStateTimes;
+
+//////////////////////////////////////////////////////////////
+
+FreeIMU              IMU_imu;
+unsigned int         IMU_step;
+Mutex                IMU_ejectionMutex;
+IMU_Ejection         IMU_eject;
+IMU_QuadStateTimes   IMU_quadState;
+Timer                IMU_dt_timer;
+DigitalOut           IMU_led (LED_IMU);
+//RtosTimer          IMU_timer; //moved to main thread stack
+
+void IMU_init(){
+    IMU_led = 1;
+    IMU_step = 0;
+    IMU_imu.init(true);
+    IMU_quadState.reset();
+}
+
+void IMU_update (void const * param){
+    ++IMU_step;
+    if(IMU_step%10 == 0){
+        //get results to be ejected.
+        IMU_ejectionMutex.lock();
+        
+            IMU_led = 1;
+            IMU_imu.getYawPitchRoll(IMU_eject.ypr);
+            IMU_eject.alt = IMU_imu.getBaroAlt();
+            IMU_eject.temp = IMU_imu.baro->getTemperature();
+            QUAD_STATE_UPDATE_DT(IMU_quadState,imu,IMU_dt_timer)
+            IMU_led = 0;
+
+        IMU_ejectionMutex.unlock();
+    }else{
+        IMU_imu.getQ(NULL);
+    }
+}
+
+void IMU_injectStateTo (QuadState & state){
+    IMU_ejectionMutex.lock();
+    
+        state.estimated_rotation_y = IMU_eject.ypr[0];
+        state.estimated_rotation_p = IMU_eject.ypr[1];
+        state.estimated_rotation_r = IMU_eject.ypr[2];
+        
+        state.target_imu_dt = IMU_quadState.target_imu_dt;
+        state.actual_imu_dt = IMU_quadState.actual_imu_dt;
+        state.average_imu_dt = IMU_quadState.average_imu_dt;
+        state.average_imu_dt_k = IMU_quadState.average_imu_dt_k;
+
+    IMU_ejectionMutex.unlock();
+} */
\ No newline at end of file