Alexandre Pirotte
/
NUCLEO_LIDAR_scan_interrupt
LUIS
fct.h
- Committer:
- pirottealex
- Date:
- 2018-10-05
- Revision:
- 0:43101a0b7a4c
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++; } } }