File content as of revision 5:6a95726e45b0:

#include "mbed.h"
#include "data_board.h"


Serial      board(PA_9, PA_10);
DigitalOut  sf_m_b(PA_11);


unsigned int flag_485_A = 0, flag_485_B = 0;
unsigned int S485num = 0;
uint16_t S485get[8] = {0};                                                    
uint16_t S485use[8] = {0};


void serial_board_isr(void)
{
    while(board.readable())
    {
        uint16_t c = board.getc();
        if(c == 0x80)
        {
            flag_485_A = 1;
            
            flag_485_B = 0;
            S485num = 0;
            for(unsigned int i = 0; i < 8; i++)
            {
                S485get[i] = 0;      
            }
            
            break;  
        }
        if(c == 0x40)
        {
            flag_485_B = 1;
        }
        
        if(flag_485_A == 1)
        {
            if((flag_485_B != 1) && (S485num < 8))
            {
                S485get[S485num] = c;    
            }
            
            S485num++;
            
            if((flag_485_B == 1) && (S485num != 9))
            {    
                flag_485_A = 0;
                flag_485_B = 0;
                S485num    = 0;     
            }
            
            if((flag_485_B == 1) && (S485num == 9))
            {
                flag_485_A = 0;
                flag_485_B = 0;
                S485num    = 0;
                
                for(unsigned int i = 0; i < 8; i++)
                {
                    S485use[i] = S485get[i];
                }  
            } 
        }
    }
}

////////////////////////////////////////////////////////////////////////////////

unsigned int Gait_num_valid = 0, Gait_now = 0;
uint16_t Gait_per_now_int = 0, COP_Y_int = 0, COP_X_int = 0;
float Gait_per_now = 0, COP_Y = 0, COP_X = 0;

unsigned int Gait_now_0before = 0;
float Gait_per_now_0before = 0, COP_Y_0before = 0, COP_X_0before = 0;

unsigned int Gait_num_valid_real = 0, Gait_now_real = 0;
float Gait_per_now_real = 0, COP_Y_real = 0, COP_X_real = 0;


void gait_decode()
{
    Gait_num_valid = S485use[0];
    
    Gait_now = S485use[1];
    
    Gait_per_now_int = (S485use[2]<<6)|S485use[3];
    Gait_per_now     = uint_to_float(Gait_per_now_int, 0.0f, 999.0f, 12);
    
    COP_Y_int = (S485use[4]<<6)|S485use[5];
    COP_Y     = uint_to_float(COP_Y_int, 0.0f, 270.0f, 12);
    
    COP_X_int = (S485use[6]<<6)|S485use[7];
    COP_X     = uint_to_float(COP_X_int, 0.0f, 100.0f, 12);
    
    if(Gait_now != 0)
        Gait_now_0before = Gait_now;
    if(Gait_now == 0)
        Gait_now = Gait_now_0before;
        
    if(Gait_per_now != 0)
        Gait_per_now_0before = Gait_per_now;
    if(Gait_per_now == 0)
        Gait_per_now = Gait_per_now_0before;    
        
    if(COP_Y != 0)
        COP_Y_0before = COP_Y;
    if(COP_Y == 0)
        COP_Y = COP_Y_0before;
        
    if(COP_X != 0)
        COP_X_0before = COP_X;
    if(COP_X == 0)
        COP_X = COP_X_0before;
        
    Gait_num_valid_real = Gait_num_valid;
    Gait_now_real = Gait_now;
    Gait_per_now_real = Gait_per_now;
    COP_Y_real = COP_Y;
    COP_X_real = COP_X;
}


void gait_clear()
{
    for(int i = 0; i < 8; i++)
        S485use[i] = 0;
}

////////////////////////////////////////////////////////////////////////////////

float ad_pf = 0, ad_df = 0;
uint16_t ad_pf_int = 0, ad_df_int = 0;
uint8_t ad_pf_forward = 0, ad_pf_backward = 0, ad_df_forward = 0, ad_df_backward = 0;
uint8_t ad_null = 0x3f;

float ad_pf_0before = 0, ad_df_0before = 0;

float ad_pf_real = 0, ad_df_real = 0;


void ad_decode()
{
    ad_pf_int = (S485use[0]<<6)|S485use[1];
    ad_df_int = (S485use[2]<<6)|S485use[3];
    
    ad_pf = uint_to_float(ad_pf_int, 0.0f, 5.0f, 12);
    ad_df = uint_to_float(ad_df_int, 0.0f, 5.0f, 12);
    
    if(ad_pf != 0)
        ad_pf_0before = ad_pf;
    if(ad_pf == 0)
        ad_pf = ad_pf_0before;
        
    if(ad_df != 0)
        ad_df_0before = ad_df;
    if(ad_df == 0)
        ad_df = ad_df_0before;
        
    ad_pf_real = ad_pf;
    ad_df_real = ad_df;
}


void ad_clear()
{
    for(int i = 0; i < 8; i++)
        S485use[i] = 0;
}