![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LUIS
fct.h
- Committer:
- pirottealex
- Date:
- 2018-10-05
- Revision:
- 0:fcc396b03bbe
File content as of revision 0:fcc396b03bbe:
#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]]); } }