File content as of revision 0:43101a0b7a4c:

#include "digital_filter.h"

#define PERIOD 0.00005
#define GET_INFO 0x50
#define GET_HEALTH 0x52
#define RESET 0x40
#define SYNC 0xA5
#define SCAN 0x20
#define STOP 0x25
#define BOFF 7

#define TTAB 1000
#define TTMES 500

#define TLONGUEUR 4000
#define TLARGEUR 4000

Serial Lidar (PC_10,PC_11);
Serial pc(USBTX,USBRX);

PwmOut LIDAR(A3);

uint16_t taille_tab=1;
uint16_t* Angle=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));
uint16_t* Distance=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));

char flag_reception=0;
uint16_t i=0;
char buffer[TTAB];

// FILTRAGE 
double coef_b[4] = {0.1448, 0.1, 0.0608, 0.0208};    // Les 4 coefficients bj
double coef_a[3] = {-1.2978, 0.7875, -0.1632};          // Les 3 coefficients ai
digital_filter mon_filtre;

void get_scan (void);
void init_lidar(void);
void get_info(void);
void get_health(void);
void get_position(void);
void init (void);
void start_scan(void);
void stop_scan(void);
void reception_lidar(void);

void init(void)
{
    
    init_digital_filter(&mon_filtre, coef_a, 3, coef_b, 4);
    filter_next_sample(&mon_filtre, 600);
    pc.baud(460800);
    pc.printf("ALIVE\n\r");
    init_lidar();    
}

void init_lidar(void)
{
    Lidar.baud(115200);
    Lidar.putc(RESET);
    wait(0.001);
    get_info();
    wait(0.001);
    get_health();
    LIDAR.period(PERIOD);
    LIDAR.pulsewidth(PERIOD*0.6);

}

void get_health(void)
{
    char info[27] = {0};
    Lidar.putc(SYNC);
    Lidar.putc(GET_HEALTH);
    for(int k=0; k<10; k++)
    {
        info[k]=Lidar.getc();
    }
    int statut=info[BOFF];
    if( statut==0) {
        pc.printf("Statut = 0 : GOOD");
    } else if( statut==1) {
        pc.printf("Statut = 1 : WARNING");
    } else if( statut==2) {
        pc.printf("Statut = 2 : ERROR");
    }
    pc.printf("\n\r");
}

void get_info(void)
{
    char info[27] = {0};
    Lidar.putc(SYNC);
    Lidar.putc(GET_INFO);
    for(int k=0; k<27; k++)
    {
        info[k]=Lidar.getc();
    }
    
    pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",info[BOFF],info[BOFF+2],info[BOFF+1]);
    for(int j=BOFF+3; j<=16+BOFF+3; j++) {
        pc.printf("%x",info[j]);
    }
    pc.printf("\n\r");
}

void get_scan (void)
{
    
    //pc.printf("coucou");
    taille_tab=1;
    char msg[TTAB] = {0};
    stop_scan();
    memcpy(&msg,&buffer,TTAB);   
    //start_scan();
    
    
    char flag=0;
    for(int ii=0; ii<TTAB-5; ii=ii+5) 
    {
        pc.printf("%d\n\r",(buffer[ii]&0x01));
        char qualite= msg[ii]>>2;
        char check = msg[ii+1]&0x01;
        
        if((msg[ii]&0x01)&&(check))
        {
            flag++;
        }
        /*if(flag==2)
        {
        pc.printf("FIN\n\r");

            taille_tab--;
            break;
        }    */    
        if((qualite>=8)&&(check)&&(flag))
        {
            uint16_t dist=((msg[ii+3]) + ((msg[ii+4]) << 8)) >>2;
            if(dist>0 && dist<700){
                
                Angle = (uint16_t*)realloc(Angle,taille_tab*sizeof(uint16_t));
                Distance = (uint16_t*)realloc(Distance,taille_tab*sizeof(uint16_t));
                Angle[taille_tab-1]=((msg[ii+1] >> 1) + (msg[ii+2] << 7)) >>6;
                Distance[taille_tab-1]=dist;
                taille_tab++;
            }
        }
    }
    uint16_t max =taille_tab,buf;
    for(uint16_t count1 = 0; count1 < max; count1++)
    {
        for(uint16_t count2 = count1; count2 < (max); count2++)
        {
            if(Angle[count2] < Angle[count1])
            {   
                buf = Angle[count2];
                Angle[count2] = Angle[count1];
                Angle[count1]= buf;
                buf = Distance[count2];
                Distance[count2] = Distance[count1];
                Distance[count1]= buf;

            }
        }
    }
    
    for (uint16_t k=0; k<taille_tab-1;k++)
    {
        pc.printf("%d, %d, %d\n\r",k,Angle[k],Distance[k]);
    }
    
    
}
 


void start_scan(void)
{
    Lidar.attach(&reception_lidar);
    flag_reception=0;
    i=0;
    Lidar.putc(SYNC);
    Lidar.putc(SCAN);
}

void stop_scan(void)
{
    Lidar.attach(0);
    Lidar.putc(SYNC);
    Lidar.putc(STOP);
    flag_reception=0;
    i=0;
}    


void reception_lidar(void)
{
    if(flag_reception==0)
    {
        
        if(i<7)
        {
            Lidar.getc();
            i++     ;       
        }
        else
        {
            pc.printf("\n\r");
            flag_reception=1;
            i=0;
        }
    }
   if(flag_reception==1)
    {
        if(i==999)
        {
            //pc.printf("%d\n\r",i);
            for (uint16_t k=0; k<TTAB-5; k++)
            {
                buffer[k]=buffer[k+5];
            }
            i=TTAB-5;
        }  
        else
        {
           buffer[i]=Lidar.getc();
           i++;
        } 
       
        
    }
}