File content as of revision 0:81f7d0ea45b6:

#include "mbed.h"
#include "ssd1306.h"
#include "codes.h"
 
DigitalOut myled(LED1);

Serial pc(USBTX,USBRX);

Serial device(PA_11, PA_12);

Ticker flipper;

SSD1306 lcd (D14, D15); // assumes default I2C address of 0x78

void flip(void);
void OBDInit(void);
void read_serial(void);

void ask_intake_pressure_absolute(void);
void ask_engine_rpm(void);
void ask_vehile_speed(void);
void ask_intake_air_temperature(void);
void ask_throttle_position(void);
void ask_oxygen_sensor(void);
void ask_fuel_rail_pressure(void);

char RESPONSE_INTAKE_PRESSURE_ABSOLUTE[64];
char RESPONSE_ENGINE_RPM[64];
char RESPONSE_VEHILE_SPEED[64];
char RESPONSE_INTAKE_AIR_TEMPERATURE[64];
char RESPONSE_THROTTLE_POSITION[64];
char RESPONSE_OXYGEN_SENSOR[64];
char RESPONSE_FUEL_RAIL_PRESSURE[64];

char c[64];

float INTAKE_PRESSURE_ABSOLUTE;
int ENGINE_RPM;
int VEHILE_SPEED;
int INTAKE_AIR_TEMPERATURE;
float THROTTLE_POSITION;
float SHORT_TERM_FUEL_TRIM;
float OXYGEN_SENSOR;
float FUEL_RAIL_PRESSURE;

int main() 
{
    flipper.attach(&flip, 1.0);
    pc.baud(115200);
        
    lcd.speed (SSD1306::Medium);  // set working frequency
    lcd.init();                   // initialize SSD1306
    lcd.set_contrast(255);
    lcd.cls();                    // clear frame buffer
    lcd.locate (3,2);             // set text cursor to line 3, column 1
    lcd.printf ("Starting up"); // print to frame buffer

    pc.printf("OBD STM32 v1.1.002");
    pc.printf("\r\n");
    
    device.baud(38400);
    OBDInit();
    
    pc.printf("OBD Inicializado");
    pc.printf("\r\n");
    
    lcd.cls();
    lcd.locate(3,0);
    lcd.printf("OBD Inicializado");
    lcd.redraw(); 
    
    wait_ms(1500);
    lcd.cls();
    lcd.redraw();
    
    int FREQ = 600;   
       
    while(1) 
    {
        ask_intake_pressure_absolute();
        wait_ms(FREQ);
        ask_engine_rpm();
        wait_ms(FREQ);
        //ask_vehile_speed();
        //wait_ms(FREQ);
        ask_intake_air_temperature();
        wait_ms(FREQ);
        ask_throttle_position();
        wait_ms(FREQ);
        ask_oxygen_sensor();
        wait_ms(FREQ);
        ask_fuel_rail_pressure();
        wait_ms(FREQ);
        
        lcd.locate(0,0);
        lcd.printf("%03.2f   bar",INTAKE_PRESSURE_ABSOLUTE);
        lcd.locate(1,0);
        lcd.printf("%04i   RPM",ENGINE_RPM);
        lcd.locate(2,0);
        lcd.printf("%03i    Km/h",VEHILE_SPEED);
        lcd.locate(3,0);
        lcd.printf("%03i    C",INTAKE_AIR_TEMPERATURE);
        lcd.locate(4,0);
        lcd.printf("%05.2f  pc",THROTTLE_POSITION);
        lcd.locate(5,0);
        lcd.printf("%04.3f  mV",OXYGEN_SENSOR);
        lcd.locate(6,0);
        lcd.printf("%06.2f bar",FUEL_RAIL_PRESSURE);
        lcd.redraw(); 
        
        //lcd.cls();
        
        
    }
}

void flip() 
{
    myled = !myled;
}

void ask_intake_pressure_absolute()
{
    int i = 0;
    
    device.printf(PID_INTAKE_PRESSURE_ABSOLUTE);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_INTAKE_PRESSURE_ABSOLUTE[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_INTAKE_PRESSURE_ABSOLUTE[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
        
    DATA_A[0]  = RESPONSE_INTAKE_PRESSURE_ABSOLUTE[i-3];
    DATA_A[1] = RESPONSE_INTAKE_PRESSURE_ABSOLUTE[i-2];
     
    int VAL_A = strtol(DATA_A,NULL,16);   
    
    //pc.printf("%i\r",VAL_A);
        
    INTAKE_PRESSURE_ABSOLUTE = (VAL_A * 0.01f) - 1.0f;
    
    pc.printf("pressao %i\r",INTAKE_AIR_TEMPERATURE);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_INTAKE_PRESSURE_ABSOLUTE[j]);
    }
}

void ask_engine_rpm()
{
    int i = 0;
    
    device.printf(PID_ENGINE_RPM);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_ENGINE_RPM[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_ENGINE_RPM[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
        
    DATA_A[0]  = RESPONSE_ENGINE_RPM[i-5];
    DATA_A[1] = RESPONSE_ENGINE_RPM[i-4];
     
    DATA_B[0]  = RESPONSE_ENGINE_RPM[i-3];
    DATA_B[1] = RESPONSE_ENGINE_RPM[i-2];
    
    int VAL_A = strtol(DATA_A,NULL,16);   
    int VAL_B = strtol(DATA_B,NULL,16);  
    
    //pc.printf("%i\r",VAL_A);
    //pc.printf("%i\r",VAL_B);   
    
    ENGINE_RPM = ((VAL_A * 256) + VAL_B) / 4;
    
    pc.printf("rpm %i\r",ENGINE_RPM);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_ENGINE_RPM[j]);
    }
}

void ask_vehile_speed()
{
    int i = 0;
    
    device.printf(PID_VEHILE_SPEED);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(c[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_VEHILE_SPEED[i] = device.getc();
        i++;
    } 
        
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
            
    DATA_A[0]  = RESPONSE_VEHILE_SPEED[i-3];
    DATA_A[1] = RESPONSE_VEHILE_SPEED[i-2];
    
    int VAL_A = strtol(DATA_A,NULL,16);   
    
    //pc.printf("%i\r",VAL_A);
           
    VEHILE_SPEED = VAL_A;
    
    pc.printf("%i kmh\r",VEHILE_SPEED);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_VEHILE_SPEED[j]);
    }
}

void ask_intake_air_temperature()
{
    int i = 0;
    
    device.printf(PID_INTAKE_AIR_TEMPERATURE);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_INTAKE_AIR_TEMPERATURE[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_INTAKE_AIR_TEMPERATURE[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
        
    DATA_A[0]  = RESPONSE_INTAKE_AIR_TEMPERATURE[i-3];
    DATA_A[1] = RESPONSE_INTAKE_AIR_TEMPERATURE[i-2];
     
    int VAL_A = strtol(DATA_A,NULL,16);   
    
    //pc.printf("%i\r",VAL_A);
        
    INTAKE_AIR_TEMPERATURE = VAL_A - 40;;
    
    pc.printf("temp ar %i\r",INTAKE_AIR_TEMPERATURE);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_INTAKE_AIR_TEMPERATURE[j]);
    }
}

void ask_throttle_position()
{
    int i = 0;
    
    device.printf(PID_THROTTLE_POSITION);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_THROTTLE_POSITION[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_THROTTLE_POSITION[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
        
    DATA_A[0]  = RESPONSE_THROTTLE_POSITION[i-3];
    DATA_A[1] = RESPONSE_THROTTLE_POSITION[i-2];
     
    int VAL_A = strtol(DATA_A,NULL,16);   
    
    //pc.printf("%i\r",VAL_A);
        
    THROTTLE_POSITION = 100.0f/255.0f;
    THROTTLE_POSITION = THROTTLE_POSITION * VAL_A;
    
    pc.printf("tbi %.2f\r",THROTTLE_POSITION);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_THROTTLE_POSITION[j]);
    }
}

void ask_oxygen_sensor()
{
    int i = 0;
    
    device.printf(PID_OXYGEN_SENSOR);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_OXYGEN_SENSOR[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_OXYGEN_SENSOR[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
        
    DATA_A[0]  = RESPONSE_OXYGEN_SENSOR[i-5];
    DATA_A[1] = RESPONSE_OXYGEN_SENSOR[i-4];
     
    DATA_B[0]  = RESPONSE_OXYGEN_SENSOR[i-3];
    DATA_B[1] = RESPONSE_OXYGEN_SENSOR[i-2];
    
    int VAL_A = strtol(DATA_A,NULL,16);   
    int VAL_B = strtol(DATA_B,NULL,16);  
    
    //pc.printf("%i\r",VAL_A);
    //pc.printf("%i\r",VAL_B);   
    
    OXYGEN_SENSOR = VAL_A / 200.0f;
    
    SHORT_TERM_FUEL_TRIM = ((100.0f / 128.0f) * VAL_B) - 100.0f;
    
    pc.printf("sonda %f\r",OXYGEN_SENSOR);
    pc.printf("trim %f\r",SHORT_TERM_FUEL_TRIM);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_OXYGEN_SENSOR[j]);
    }
}

void ask_fuel_rail_pressure()
{
    int i = 0;
    
    device.printf(PID_FUEL_RAIL_PRESSURE);
    device.printf(CR);
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(RESPONSE_FUEL_RAIL_PRESSURE[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        RESPONSE_FUEL_RAIL_PRESSURE[i] = device.getc();
        i++;
    } 
    
    char DATA_A[2];
    char DATA_B[2];
    char DATA_C[2];
    char DATA_D[2];
                
    DATA_A[0]  = RESPONSE_FUEL_RAIL_PRESSURE[i-5];
    DATA_A[1] = RESPONSE_FUEL_RAIL_PRESSURE[i-4];
     
    DATA_B[0]  = RESPONSE_FUEL_RAIL_PRESSURE[i-3];
    DATA_B[1] = RESPONSE_FUEL_RAIL_PRESSURE[i-2];
    
    int VAL_A = strtol(DATA_A,NULL,16);   
    int VAL_B = strtol(DATA_B,NULL,16);   
    
    //pc.printf("%i\r",VAL_A);  
    
    FUEL_RAIL_PRESSURE = ((VAL_A * 256) + VAL_B) * 0.1f;
    
    pc.printf("alta %f\r",FUEL_RAIL_PRESSURE);
    
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",RESPONSE_FUEL_RAIL_PRESSURE[j]);
    }
}

void read_serial()
{
    int i = 0;
          
    while(!device.readable()){} //fica esperando chegar algo na serial
    while(c[i-1]!= '\r')        //chegou algo! vamos ler até o CR
    {  
        c[i] = device.getc();
        //pc.printf("%c",c[i]);
        i++;
    } 
    for(int j = 0; j < i; j++)
    { 
        pc.printf("%c",c[j]);
    }
    i=0;  //zeramos o contador de posicao
}

void OBDInit()
{
    lcd.locate (4,0);
    lcd.printf (".");
    lcd.redraw();
    device.printf(SET_ALL_TO_DEFAULT);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,1);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
                
    lcd.locate (4,2);
    lcd.printf (".");
    lcd.redraw();            
    device.printf(RESET_ALL);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,3);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,4);
    lcd.printf (".");
    lcd.redraw();   
    device.printf(LINEFEED_OFF);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,5);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,6);
    lcd.printf (".");
    lcd.redraw(); 
    device.printf(PRINTING_SPACES_OFF);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,7);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,8);
    lcd.printf (".");
    lcd.redraw();
    device.printf(MEMORY_OFF);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,9);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,10);
    lcd.printf (".");
    lcd.redraw();        
    device.printf(SET_TIMEOUT_256);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,11);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,12);
    lcd.printf (".");
    lcd.redraw();    
    device.printf(TRY_PROTOCOL_0);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,13);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    lcd.locate (4,14);
    lcd.printf (".");
    lcd.redraw();    
    device.printf(ECHO_OFF);
    device.printf(CR);
    wait_ms(500);
    lcd.locate (4,15);
    lcd.printf (".");
    lcd.redraw();
    wait_ms(500);
    
    device.printf("ATLP");
    device.printf(CR);
    wait_ms(500);
}