File content as of revision 8:f23601373e8b:

#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);

    }
}