File content as of revision 6:9af875ef7b30:

#include "mbed.h"

Serial ld(PC_10,PC_11,115200);

PwmOut pwm_lidar(PC_8);

void Interrupt_ldRX(void);
DigitalOut myled1(LED1);
uint16_t data[4]; // donnees traitees
uint16_t prevData[4] = {1, 10, 0, 1000}; // donnees traitees precedentes
int newData = 0; // flag indiquant de nouvelles donnees
bool newRadar = false; // flag indiquant un nouveau tour effectué 

// tableau tps reel : Radar[angle] = distance;
int Radar[360]; // de 0 a 359
int maxDist = 5000; // distance max visible par le lidar
int minDist = 300; // distance min visible par le lidar

// commandes
char ldStart[3] = {0xA5, 0x20,0};
char ldStop[3] = {0xA5, 0x25, 0};
char ldReset[3] = {0xA5, 0x40, 0};
// donnees brutes recues au debut
char start_data_received[7];
    
void recuperer_start(char* data_received){
    // recupere le premier paquet, qui est different des suivants
    int i=0;
    while(i<7){
        data_received[i] = ld.getc();
        if((i!=0)||(i==0 && data_received[i]==0xA5)){
            i++;
        }
    }
}


void ldConvert(uint8_t* data_received, uint16_t* data){
    // convertit les donnees recues : 5 paquets de 2 hexas
    uint8_t qualityHex;
    uint16_t checkS;
    uint16_t quality;
    // angle + checkC
    uint16_t angleHex1 ;
    uint16_t angleHex2;
    uint16_t checkC ;
    uint8_t distanceHex2,distanceHex1;
    uint16_t angle,distance;
    myled1=!myled1;
    // quality + checkS
    qualityHex = data_received[0];
    checkS = ((qualityHex&0x02)>>1)!=(qualityHex&0x01);
    quality = (qualityHex&0xFC)>>2;
    // angle + checkC
    angleHex1 = data_received[1];
    angleHex2 = data_received[2];
    checkC = (angleHex1&0x01);
    angle = (((uint16_t)angleHex2) <<1) + ((uint16_t)angleHex1>>7);
    angle = angle - 180; // Decalage par rapport au centre du lidar et de la voiture
    // distance
    distanceHex1 = data_received[3];
    distanceHex2 = data_received[4];
    distance = (((uint16_t)distanceHex2) <<6) + (distanceHex1>>2);
    data[0] = (checkS && checkC);
    data[1] = quality;
    if(angle >= 360){
        angle = 0;
    }
    data[2] = angle;
    data[3] = distance;
}


void addRadar(uint16_t* data, uint16_t* prevData){
    // ajoute les donnees du bloc data dans le radar actuel
    int cA = data[2];
    int cD = data[3];
    int pA = prevData[2];
    int pD = prevData[3];
    // pente de la droite d interpolation lineaire
    float coeff;
    // si on a pas fait un tour complet
    if(cA >= pA){

        coeff = (float)(cD-pD)/(float)(cA-pA);
        int ka = pA + 1;
        while(ka < cA){
            float distance = (float)pD + coeff*(float)(ka - pA);
            if(distance > maxDist){
                distance = maxDist;
            }else if(distance < minDist){
                distance = minDist;
            }
            Radar[ka] = distance;
            ++ka;
        }
        Radar[cA] = cD;
    }
    else{ //Pa>ca
         // si on a fait un tour
        newRadar = true;
        int ka = pA + 1;
        coeff = (float)(cD-pD)/(float)(cA+360-pA);
        // fin du Radar
        while(ka < 360){
            float distance = (float)pD + coeff*(float)(ka - pA);
            if(distance > maxDist){
                distance = maxDist;
            }
            else if(distance < minDist){
                distance = minDist;
            }
            Radar[ka] = distance;
            ++ka;
        }
        // début du Radar
        ka = 0;
        while(ka <= cA){
            Radar[ka] = Radar[359] + coeff*(ka + 1);
            ++ka;
        }
    }
}

void Interrupt_ldRX(void){
    // Toutes les 500 microsecondes
    static int i=0; // indice dans un paquet de donnees (5 octets)
    static uint8_t data_received_temp[5];
    uint16_t data[4];
    //static uint16_t prevData[4];
    //static int newData = 0;
    
    data_received_temp[i] = ld.getc(); // on recup l info
    
    // si on atteint le fin du paquet de donnees
    if(i==4){
        // on met les donnees utilisables dans data
        ldConvert(data_received_temp, data);
        // on verifie le bit de check et la qualite
        if (data[0] &&(data[1]>=8)){
            // data -> prevData
            addRadar(data,prevData);
            for(int k=0; k<4; k++){
                prevData[k] = data[k];
            }
            newData = 1;
        }
        
        if (data[0] &&(data[1]<8)){
            //addRadar(Radar,data,prevData);
            //for(int k=0; k<3; k++){
            //    prevData[k] = data[k];
            //}
        }
    }
    i=(i+1)%5;
}

void setup_lidar(){
    // start lidar
    wait(0.5);
    pwm_lidar.period_us(40);
    pwm_lidar.write(0.5);
    wait(1);
    ld.puts(ldStart);
    // on recupere le paquet de depart
    recuperer_start(start_data_received);
    wait(0.1);
    // fonction d interruption
    ld.attach(Interrupt_ldRX,Serial::RxIrq);
    }