Diff: sm_mpu.cpp

Revision:

8:f23601373e8b

Child:

9:1b54bac6d9a7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sm_mpu.cpp	Wed Oct 02 22:25:12 2019 +0000
@@ -0,0 +1,96 @@
+#include "sm_mpu.h"
+#include "odom.h"
+#include "sm_esc.h"
+
+MPU6050 s_MPU_mpu;
+Timer t_mpuLoggingTimer;
+int i_mpuLogging_duration;
+bool b_MPU_logFlag = false;
+int mpu_init_try = 0;
+
+int16_t ax, ay, az;
+double ax_g,ay_g,az_g;
+int16_t gx, gy, gz;
+
+E_STATE_MPU e_state_MPU;
+
+bool mpu6050TestResult ;
+
+void init_sm_mpu()
+{
+    e_state_MPU = INIT_MPU;
+}
+
+void update_sm_mpu()
+{
+    E_STATE_MPU e_next_state = e_state_MPU;;
+
+    switch(e_state_MPU) {
+        case INIT_MPU:
+                e_next_state = RUNNING_MPU;
+            break;
+        case RUNNING_MPU:
+
+            break;
+
+        default:
+            break;
+    }
+
+    e_state_MPU = e_next_state;
+
+}
+
+void output_sm_mpu()
+{
+    switch(e_state_MPU) {
+        case INIT_MPU:
+            mpu6050TestResult = false;
+            rs_UTILS_pc.printf("MPU INITIALIZING");
+            s_MPU_mpu.initialize();
+            rs_UTILS_odroid.printf("MPU6050 testConnection \r\n");
+
+            mpu6050TestResult = s_MPU_mpu.testConnection();
+            if(mpu6050TestResult) {
+                rs_UTILS_odroid.printf("MPU6050 test passed \r\n");
+            } else {
+                rs_UTILS_odroid.printf("MPU6050 test failed \r\n");
+            }
+            break;
+        case RUNNING_MPU:
+            //logging mpu data
+            mpu_log_notify();
+            break;
+
+        default:
+            break;
+    }
+
+}
+
+void mpu_log_start(int duration_ms)
+{
+    b_MPU_logFlag = true;
+    t_mpuLoggingTimer.start();
+    i_mpuLogging_duration = duration_ms;
+    t_mpuLoggingTimer.reset();
+}
+
+void mpu_log_notify()
+{
+    if(t_mpuLoggingTimer.read_ms() > i_mpuLogging_duration) {
+        b_MPU_logFlag = false;
+        t_mpuLoggingTimer.stop();
+        t_mpuLoggingTimer.reset();
+    }
+    if(b_MPU_logFlag && t_mpuLoggingTimer.read_ms() < i_mpuLogging_duration) {
+        s_MPU_mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        //correction pour avoir une acceleration coef empirique
+        ax_g = ax /1658.0 ;
+        ay_g = ay /1658.0 ;
+        az_g = az /1658.0 ;
+
+        //rs_UTILS_pc.printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;gyr X =%d; gyr Y =%d; gyr Z=%d;speed: %f;odom section %f; odom global %f\n\r",ax_g,ay_g,az_g,gx,gy,gz,d_ODOM_speed_mps,d_ODOM_distFromSectionStart_m,d_ODOM_distFromGlobalStart_m);
+
+    }
+}
\ No newline at end of file