![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LUIS
Diff: fct.h
- Revision:
- 0:43101a0b7a4c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fct.h Fri Oct 05 14:55:30 2018 +0000 @@ -0,0 +1,232 @@ +#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++; + } + + + } +} +