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

Committer:
GaspardD
Date:
2019-10-04
Revision:
10:e63fe4080760
Parent:
9:1b54bac6d9a7
Child:
12:58ad06f9847d

File content as of revision 10:e63fe4080760:

#include "sm_mpu.h"
#include "odom.h"
#include "sm_esc.h"

MPU6050 s_MPU_mpu;

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_LOG_pc.printf("MPU INITIALIZING");
            s_MPU_mpu.initialize();
            //rs_LOG_odroid.printf("MPU6050 testConnection \r\n");

            mpu6050TestResult = s_MPU_mpu.testConnection();
            if(mpu6050TestResult) {
                //rs_LOG_odroid.printf("MPU6050 test passed \r\n");
            } else {
                //rs_LOG_odroid.printf("MPU6050 test failed \r\n");
            }
            break;
        case RUNNING_MPU:
            //logging mpu data
            
            /*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_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);
            */
            break;

        default:
            break;
    }

}