File content as of revision 9:1b54bac6d9a7:

#include "odom.h"
#include "utils.h"

InterruptIn b_ODOM_odom(PF_8);

double d_ODOM_distFromSectionStart_m = 0.0;
double d_ODOM_distFromGlobalStart_m = 0.0;

double d_ODOM_speed_mps = 0.0;
double d_ODOM_accel_mpsps = 0.0;

double d_ODOM_computed_pos_m = 0.0;

int i_ODOM_last_time_ms = 0 ;
int i_last_time_ms = 0 ;
int i_ODOM_time_ms = 0;
int i_time_ms;

int now;
double d_distance_since_it_m;

void it_odom()
{
    //rs_LOG_pc.printf("debut it odom\r\n");
    if(t_utils_timerSinceStart.read_ms() != i_ODOM_time_ms)
    {
        i_ODOM_last_time_ms = i_ODOM_time_ms;
        i_ODOM_time_ms = t_utils_timerSinceStart.read_ms();
        d_ODOM_distFromSectionStart_m += d_CHASSIS_dist_one_step_odom;
        d_ODOM_distFromGlobalStart_m += d_CHASSIS_dist_one_step_odom;
    }
    return;
}

void update_speed()
{
    i_last_time_ms = i_ODOM_last_time_ms;
    i_time_ms = i_ODOM_time_ms;

    d_ODOM_speed_mps = d_CHASSIS_dist_one_step_odom/((i_time_ms - i_last_time_ms)/1000.0) ;
    //rs_LOG_pc.printf("vitesse : %f mps; i_time_ms: %d; i_last_time_ms: %d \r\n",d_ODOM_speed_mps,i_time_ms, i_last_time_ms);
    
    now = t_utils_timerSinceStart.read_ms();
    d_distance_since_it_m = d_ODOM_speed_mps * ((now - i_time_ms)/1000.0);

    //rs_LOG_pc.printf("d_distance_since_it_m = %f =  %f * ((%ld - %ld)/1000)\r\n",d_distance_since_it_m,d_ODOM_speed_mps,now,i_last_time_ms);

    // distance par rapport au debut a l'instant T
    d_ODOM_computed_pos_m = d_ODOM_distFromGlobalStart_m + d_distance_since_it_m;
    
    
    //rs_LOG_pc.printf("computed dist since start: %f  since it: %f\r\n",d_ODOM_computed_pos_m,d_distance_since_it_m);
}