File content as of revision 0:ce6055872f4e:

#include "car_config.hpp"
#include "mbed.h"
#include "rtos.h"

extern can_cmd_body_t can_cmd_body;
extern can_sts_body_t can_sts_body;

AnalogIn back_r (p19);
AnalogIn back_l (p20);
I2C i2c(p28, p27);
char addr;

uint8 read_back_distance (AnalogIn* side);
void srf10_start ();
void srf10_change_address ();
uint16 srf10_read ();


#define k_5 12466.0
#define k_4 -23216.0
#define k_3 14974.0
#define k_2 -3585.0
#define k_1 19.0
#define k_0 96.0

void init_body()
{
    srf10_start();
    srf10_change_address();
}

void stop_body()
{
    printf("MISSING BODY!!!\r\n");
}

void thread_body (void const *args)
{
    while(1) {
        printf("BODY\r\n");
        if (can_msg_is_missing(CAN_MISSING_BODY_ID))
            stop_body();
        else {
            if (can_cmd_body.flag == CAN_FLAG_RECEIVED) {
                int r = can_cmd_body.payload.msg.light_r;
                int l = can_cmd_body.payload.msg.light_l;
                int c = can_cmd_body.payload.msg.light_c;
                printf("READ CMD_BODY: %d %d %d\r\n", r, l, c);
                can_cmd_body.flag = CAN_FLAG_EMPTY;    
            }
            static int x = 0;
            can_sts_body.payload.msg.eye_back_l = read_back_distance(&back_l);
            can_sts_body.payload.msg.eye_back_r = read_back_distance(&back_r);
            can_sts_body.payload.msg.eye_front = srf10_read();
            can_sts_body.payload.buf[0] = x;
            x++;
        }
        Thread::wait(1000);
    }
}

uint8 read_back_distance (AnalogIn* side)
{
    float x = side->read();
    float res = 0.0;
    res += k_5 * (x * x * x * x * x);
    res += k_4 * (x * x * x * x);
    res += k_3 * (x * x * x);
    res += k_2 * (x * x);
    res += k_1 *  x;
    res += k_0;
    if (res<0)
        return 0;
    if (res>255)
        return res;
    return (uint8)res;
}

void srf10_start ()
{
    char cmd[2];

    addr = 0xE0;

    //set range: register x 43mm + 43mm
    cmd[0] = 0x02;
    cmd[1] = 0x30; //register = 48*43mm+43mm=2107mm
    i2c.write(addr, cmd, 2);

    cmd[0] = 0x01; //Receiver gain register
    cmd[1] = 0x10; //maximum gain
    i2c.write(addr, cmd, 2);
}

void srf10_change_address ()
{
    char cmd[2];

    cmd[0] = 0x00;
    cmd[1] = 0xA0;
    i2c.write(addr, cmd, 2);
    cmd[0] = 0x00;
    cmd[1] = 0xAA;
    i2c.write(addr, cmd, 2);
    cmd[0] = 0x00;
    cmd[1] = 0xA5;
    i2c.write(addr, cmd, 2);
    cmd[0] = 0x00;
    cmd[1] = 0xF2;
    i2c.write(addr, cmd, 2);

    addr = 0xF2;
}

uint16 srf10_read ()
{
    char cmd[2];
    char echo[2];

    cmd[0] = 0x00;                      // Command register
    cmd[1] = 0x51;                      // Ranging results in cm
    i2c.write(addr, cmd, 2);          // Send ranging burst

    wait(0.07);                         // Wait for return echo

    cmd[0] = 0x02;                        // Address of first echo
    i2c.write(addr, cmd, 1, 1);         // Send address of first echo
    i2c.read(addr, echo, 2);            // Read two-byte echo result

    return (echo[0]<<8)+echo[1];
}