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@9:1b54bac6d9a7, 2019-10-03 (annotated)

Committer:
GaspardD
Date:
Thu Oct 03 23:28:56 2019 +0000
Revision:
9:1b54bac6d9a7
Parent:
8:f23601373e8b
Child:
10:e63fe4080760
a tester ;

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
GaspardD 8:f23601373e8b 7 int16_t ax, ay, az;
GaspardD 8:f23601373e8b 8 double ax_g,ay_g,az_g;
GaspardD 8:f23601373e8b 9 int16_t gx, gy, gz;
GaspardD 8:f23601373e8b 10
GaspardD 8:f23601373e8b 11 E_STATE_MPU e_state_MPU;
GaspardD 8:f23601373e8b 12
GaspardD 8:f23601373e8b 13 bool mpu6050TestResult ;
GaspardD 8:f23601373e8b 14
GaspardD 8:f23601373e8b 15 void init_sm_mpu()
GaspardD 8:f23601373e8b 16 {
GaspardD 8:f23601373e8b 17 e_state_MPU = INIT_MPU;
GaspardD 8:f23601373e8b 18 }
GaspardD 8:f23601373e8b 19
GaspardD 8:f23601373e8b 20 void update_sm_mpu()
GaspardD 8:f23601373e8b 21 {
GaspardD 8:f23601373e8b 22 E_STATE_MPU e_next_state = e_state_MPU;;
GaspardD 8:f23601373e8b 23
GaspardD 8:f23601373e8b 24 switch(e_state_MPU) {
GaspardD 8:f23601373e8b 25 case INIT_MPU:
GaspardD 9:1b54bac6d9a7 26 e_next_state = RUNNING_MPU;
GaspardD 8:f23601373e8b 27 break;
GaspardD 8:f23601373e8b 28 case RUNNING_MPU:
GaspardD 8:f23601373e8b 29
GaspardD 8:f23601373e8b 30 break;
GaspardD 8:f23601373e8b 31
GaspardD 8:f23601373e8b 32 default:
GaspardD 8:f23601373e8b 33 break;
GaspardD 8:f23601373e8b 34 }
GaspardD 8:f23601373e8b 35
GaspardD 8:f23601373e8b 36 e_state_MPU = e_next_state;
GaspardD 8:f23601373e8b 37
GaspardD 8:f23601373e8b 38 }
GaspardD 8:f23601373e8b 39
GaspardD 8:f23601373e8b 40 void output_sm_mpu()
GaspardD 8:f23601373e8b 41 {
GaspardD 8:f23601373e8b 42 switch(e_state_MPU) {
GaspardD 8:f23601373e8b 43 case INIT_MPU:
GaspardD 8:f23601373e8b 44 mpu6050TestResult = false;
GaspardD 9:1b54bac6d9a7 45 rs_LOG_pc.printf("MPU INITIALIZING");
GaspardD 8:f23601373e8b 46 s_MPU_mpu.initialize();
GaspardD 8:f23601373e8b 47 rs_UTILS_odroid.printf("MPU6050 testConnection \r\n");
GaspardD 8:f23601373e8b 48
GaspardD 8:f23601373e8b 49 mpu6050TestResult = s_MPU_mpu.testConnection();
GaspardD 8:f23601373e8b 50 if(mpu6050TestResult) {
GaspardD 8:f23601373e8b 51 rs_UTILS_odroid.printf("MPU6050 test passed \r\n");
GaspardD 8:f23601373e8b 52 } else {
GaspardD 8:f23601373e8b 53 rs_UTILS_odroid.printf("MPU6050 test failed \r\n");
GaspardD 8:f23601373e8b 54 }
GaspardD 8:f23601373e8b 55 break;
GaspardD 8:f23601373e8b 56 case RUNNING_MPU:
GaspardD 8:f23601373e8b 57 //logging mpu data
GaspardD 9:1b54bac6d9a7 58
GaspardD 9:1b54bac6d9a7 59 /*s_MPU_mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
GaspardD 9:1b54bac6d9a7 60 //correction pour avoir une acceleration coef empirique
GaspardD 9:1b54bac6d9a7 61 ax_g = ax /1658.0 ;
GaspardD 9:1b54bac6d9a7 62 ay_g = ay /1658.0 ;
GaspardD 9:1b54bac6d9a7 63 az_g = az /1658.0 ;
GaspardD 9:1b54bac6d9a7 64
GaspardD 9:1b54bac6d9a7 65 rs_LOG_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 9:1b54bac6d9a7 66 */
GaspardD 8:f23601373e8b 67 break;
GaspardD 8:f23601373e8b 68
GaspardD 8:f23601373e8b 69 default:
GaspardD 8:f23601373e8b 70 break;
GaspardD 8:f23601373e8b 71 }
GaspardD 8:f23601373e8b 72
GaspardD 8:f23601373e8b 73 }