Alexandre Pirotte
/
NUCLEO_LIDAR
LUIS
fct.h
- Committer:
- pirottealex
- Date:
- 2018-10-05
- Revision:
- 0:96bdb0d08de1
File content as of revision 0:96bdb0d08de1:
#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 Serial Lidar (PC_10,PC_11); Serial pc(USBTX,USBRX); PwmOut LIDAR(A3); char msg[50] = {0}; int i=0; int flag=0; void get_health(void) { Lidar.putc(SYNC); Lidar.putc(GET_HEALTH); wait(0.1); int statut=msg[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"); for(int j=0; j<=50;j++) { msg[i]=0; } i=0; } void get_info(void) { Lidar.putc(SYNC); Lidar.putc(GET_INFO); wait(0.1); pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",msg[BOFF],msg[BOFF+2],msg[BOFF+1]); for(int j=BOFF+3; j<=16+BOFF+3;j++) { pc.printf("%x",msg[j]); } pc.printf("\n\r"); for(int j=0; j<=50;j++) { msg[i]=0; } i=0; } void reception (void) { msg[i] =Lidar.getc(); /*if((flag==0)&&(i==6)) { pc.printf("%d ,%d ,%d,%d ,%d, %d ,%d\n\r",msg[0],msg[1],msg[2],msg[3],msg[4], msg[5], msg[6]); flag=1; i=-1; for(int j=0; j<=50;j++) { msg[i]=0; } } //pc.printf("%d",c); if((i==4) && (flag==1)) { if(msg[1] & 0xb1) int qualite= msg[0]>>2; if(qualite>=2) { int angle=((msg[1] >> 1) + (msg[2] << 7)) / 64.0; int dist=((msg[3]) + ((msg[4]) << 8)) / 4.0; pc.printf("%d ,%d ,%d\n\r",qualite,angle,dist); } //pc.printf("%d",i); i=-1; for(int j=0; j<=50;j++) { msg[i]=0; } }*/ i++; }