TRR 2018 / Mbed 2 deprecated biniou-TRR2019-DLVV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050

sm_mpu.cpp@8:f23601373e8b, 2019-10-02 (annotated)

Committer:
GaspardD
Date:
Wed Oct 02 22:25:12 2019 +0000
Revision:
8:f23601373e8b
Child:
9:1b54bac6d9a7
angle correction working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 8:f23601373e8b 1 #include "sm_mpu.h"
GaspardD 8:f23601373e8b 2 #include "odom.h"
GaspardD 8:f23601373e8b 3 #include "sm_esc.h"
GaspardD 8:f23601373e8b 4
GaspardD 8:f23601373e8b 5 MPU6050 s_MPU_mpu;
GaspardD 8:f23601373e8b 6 Timer t_mpuLoggingTimer;
GaspardD 8:f23601373e8b 7 int i_mpuLogging_duration;
GaspardD 8:f23601373e8b 8 bool b_MPU_logFlag = false;
GaspardD 8:f23601373e8b 9 int mpu_init_try = 0;
GaspardD 8:f23601373e8b 10
GaspardD 8:f23601373e8b 11 int16_t ax, ay, az;
GaspardD 8:f23601373e8b 12 double ax_g,ay_g,az_g;
GaspardD 8:f23601373e8b 13 int16_t gx, gy, gz;
GaspardD 8:f23601373e8b 14
GaspardD 8:f23601373e8b 15 E_STATE_MPU e_state_MPU;
GaspardD 8:f23601373e8b 16
GaspardD 8:f23601373e8b 17 bool mpu6050TestResult ;
GaspardD 8:f23601373e8b 18
GaspardD 8:f23601373e8b 19 void init_sm_mpu()
GaspardD 8:f23601373e8b 20 {
GaspardD 8:f23601373e8b 21 e_state_MPU = INIT_MPU;
GaspardD 8:f23601373e8b 22 }
GaspardD 8:f23601373e8b 23
GaspardD 8:f23601373e8b 24 void update_sm_mpu()
GaspardD 8:f23601373e8b 25 {
GaspardD 8:f23601373e8b 26 E_STATE_MPU e_next_state = e_state_MPU;;
GaspardD 8:f23601373e8b 27
GaspardD 8:f23601373e8b 28 switch(e_state_MPU) {
GaspardD 8:f23601373e8b 29 case INIT_MPU:
GaspardD 8:f23601373e8b 30 e_next_state = RUNNING_MPU;
GaspardD 8:f23601373e8b 31 break;
GaspardD 8:f23601373e8b 32 case RUNNING_MPU:
GaspardD 8:f23601373e8b 33
GaspardD 8:f23601373e8b 34 break;
GaspardD 8:f23601373e8b 35
GaspardD 8:f23601373e8b 36 default:
GaspardD 8:f23601373e8b 37 break;
GaspardD 8:f23601373e8b 38 }
GaspardD 8:f23601373e8b 39
GaspardD 8:f23601373e8b 40 e_state_MPU = e_next_state;
GaspardD 8:f23601373e8b 41
GaspardD 8:f23601373e8b 42 }
GaspardD 8:f23601373e8b 43
GaspardD 8:f23601373e8b 44 void output_sm_mpu()
GaspardD 8:f23601373e8b 45 {
GaspardD 8:f23601373e8b 46 switch(e_state_MPU) {
GaspardD 8:f23601373e8b 47 case INIT_MPU:
GaspardD 8:f23601373e8b 48 mpu6050TestResult = false;
GaspardD 8:f23601373e8b 49 rs_UTILS_pc.printf("MPU INITIALIZING");
GaspardD 8:f23601373e8b 50 s_MPU_mpu.initialize();
GaspardD 8:f23601373e8b 51 rs_UTILS_odroid.printf("MPU6050 testConnection \r\n");
GaspardD 8:f23601373e8b 52
GaspardD 8:f23601373e8b 53 mpu6050TestResult = s_MPU_mpu.testConnection();
GaspardD 8:f23601373e8b 54 if(mpu6050TestResult) {
GaspardD 8:f23601373e8b 55 rs_UTILS_odroid.printf("MPU6050 test passed \r\n");
GaspardD 8:f23601373e8b 56 } else {
GaspardD 8:f23601373e8b 57 rs_UTILS_odroid.printf("MPU6050 test failed \r\n");
GaspardD 8:f23601373e8b 58 }
GaspardD 8:f23601373e8b 59 break;
GaspardD 8:f23601373e8b 60 case RUNNING_MPU:
GaspardD 8:f23601373e8b 61 //logging mpu data
GaspardD 8:f23601373e8b 62 mpu_log_notify();
GaspardD 8:f23601373e8b 63 break;
GaspardD 8:f23601373e8b 64
GaspardD 8:f23601373e8b 65 default:
GaspardD 8:f23601373e8b 66 break;
GaspardD 8:f23601373e8b 67 }
GaspardD 8:f23601373e8b 68
GaspardD 8:f23601373e8b 69 }
GaspardD 8:f23601373e8b 70
GaspardD 8:f23601373e8b 71 void mpu_log_start(int duration_ms)
GaspardD 8:f23601373e8b 72 {
GaspardD 8:f23601373e8b 73 b_MPU_logFlag = true;
GaspardD 8:f23601373e8b 74 t_mpuLoggingTimer.start();
GaspardD 8:f23601373e8b 75 i_mpuLogging_duration = duration_ms;
GaspardD 8:f23601373e8b 76 t_mpuLoggingTimer.reset();
GaspardD 8:f23601373e8b 77 }
GaspardD 8:f23601373e8b 78
GaspardD 8:f23601373e8b 79 void mpu_log_notify()
GaspardD 8:f23601373e8b 80 {
GaspardD 8:f23601373e8b 81 if(t_mpuLoggingTimer.read_ms() > i_mpuLogging_duration) {
GaspardD 8:f23601373e8b 82 b_MPU_logFlag = false;
GaspardD 8:f23601373e8b 83 t_mpuLoggingTimer.stop();
GaspardD 8:f23601373e8b 84 t_mpuLoggingTimer.reset();
GaspardD 8:f23601373e8b 85 }
GaspardD 8:f23601373e8b 86 if(b_MPU_logFlag && t_mpuLoggingTimer.read_ms() < i_mpuLogging_duration) {
GaspardD 8:f23601373e8b 87 s_MPU_mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
GaspardD 8:f23601373e8b 88 //correction pour avoir une acceleration coef empirique
GaspardD 8:f23601373e8b 89 ax_g = ax /1658.0 ;
GaspardD 8:f23601373e8b 90 ay_g = ay /1658.0 ;
GaspardD 8:f23601373e8b 91 az_g = az /1658.0 ;
GaspardD 8:f23601373e8b 92
GaspardD 8:f23601373e8b 93 //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);
GaspardD 8:f23601373e8b 94
GaspardD 8:f23601373e8b 95 }
GaspardD 8:f23601373e8b 96 }