Alexandre Pirotte
/
NUCLEO_LIDAR_scan
LUIS
Diff: fct.h
- Revision:
- 0:fcc396b03bbe
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fct.h Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,204 @@ +#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 1800 +#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)); + +// 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 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) +{ + taille_tab=1; + char msg[TTAB] = {0}; + + Lidar.putc(SYNC); + Lidar.putc(SCAN); + + for(uint16_t k=0; k<TTAB; k++) + { + msg[k]=Lidar.getc(); + } + Lidar.putc(SYNC); + Lidar.putc(STOP); + + char flag=0; + for(int i=10; i<TTAB-1; i=i+5) + { + char qualite= msg[i-3]>>2; + char check = msg[i-2]&0x01; + + if((msg[i-3]&0x01)&&(check)) + { + flag++; + } + if(flag==2) + { + taille_tab--; + break; + } + if((qualite>=8)&&(check)&&(flag)) + { + uint16_t dist=((msg[i]) + ((msg[i+1]) << 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[i-2] >> 1) + (msg[i-1] << 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 get_position(void) +{ + get_scan(); + pc.printf("%d\n\r", taille_tab); + uint16_t dist_filtre[taille_tab]; + int16_t dist_deriv[taille_tab-1]; + uint8_t indice[20]; + uint8_t indice_count=0; + for (uint16_t j=0; j<taille_tab;j++) + { + dist_filtre[j]=filter_next_sample(&mon_filtre, Distance[j]); + //pc.printf("%d, %d, %d\n\r",Angle[j], Distance[j], dist_filtre[j]); + //pc.printf("%d, %d\n\r",j, taille_tab); + } + for (uint16_t j=1; j<taille_tab;j++) + { + dist_deriv[j-1]=(int16_t)(dist_filtre[j-1]-dist_filtre[j]); + //pc.printf("%d\n\r",dist_deriv[j-1]); + } + + for (uint16_t j=0; j<taille_tab-2;j++) + { + if((dist_deriv[j]<=0)&&(dist_deriv[j+1]>0)&&(abs(dist_deriv[j]-dist_deriv[j+1]))>5) + { + indice[indice_count]=j; + indice_count++; + } + } + + for (uint16_t j=0; j<indice_count;j++) + { + pc.printf("indice %d Angle %d\n\r", indice[j],Angle[indice[j]]); + } +} + + +