Alexandre Pirotte
/
NUCLEO_LIDAR
LUIS
fct.h@0:96bdb0d08de1, 2018-10-05 (annotated)
- Committer:
- pirottealex
- Date:
- Fri Oct 05 14:54:54 2018 +0000
- Revision:
- 0:96bdb0d08de1
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pirottealex | 0:96bdb0d08de1 | 1 | |
pirottealex | 0:96bdb0d08de1 | 2 | #define PERIOD 0.00005 |
pirottealex | 0:96bdb0d08de1 | 3 | #define GET_INFO 0x50 |
pirottealex | 0:96bdb0d08de1 | 4 | #define GET_HEALTH 0x52 |
pirottealex | 0:96bdb0d08de1 | 5 | #define RESET 0x40 |
pirottealex | 0:96bdb0d08de1 | 6 | #define SYNC 0xA5 |
pirottealex | 0:96bdb0d08de1 | 7 | #define SCAN 0x20 |
pirottealex | 0:96bdb0d08de1 | 8 | #define STOP 0x25 |
pirottealex | 0:96bdb0d08de1 | 9 | #define BOFF 7 |
pirottealex | 0:96bdb0d08de1 | 10 | |
pirottealex | 0:96bdb0d08de1 | 11 | Serial Lidar (PC_10,PC_11); |
pirottealex | 0:96bdb0d08de1 | 12 | Serial pc(USBTX,USBRX); |
pirottealex | 0:96bdb0d08de1 | 13 | |
pirottealex | 0:96bdb0d08de1 | 14 | PwmOut LIDAR(A3); |
pirottealex | 0:96bdb0d08de1 | 15 | |
pirottealex | 0:96bdb0d08de1 | 16 | char msg[50] = {0}; |
pirottealex | 0:96bdb0d08de1 | 17 | int i=0; |
pirottealex | 0:96bdb0d08de1 | 18 | int flag=0; |
pirottealex | 0:96bdb0d08de1 | 19 | |
pirottealex | 0:96bdb0d08de1 | 20 | |
pirottealex | 0:96bdb0d08de1 | 21 | |
pirottealex | 0:96bdb0d08de1 | 22 | void get_health(void) |
pirottealex | 0:96bdb0d08de1 | 23 | { |
pirottealex | 0:96bdb0d08de1 | 24 | Lidar.putc(SYNC); |
pirottealex | 0:96bdb0d08de1 | 25 | Lidar.putc(GET_HEALTH); |
pirottealex | 0:96bdb0d08de1 | 26 | wait(0.1); |
pirottealex | 0:96bdb0d08de1 | 27 | int statut=msg[BOFF]; |
pirottealex | 0:96bdb0d08de1 | 28 | if( statut==0) |
pirottealex | 0:96bdb0d08de1 | 29 | { |
pirottealex | 0:96bdb0d08de1 | 30 | pc.printf("Statut = 0 : GOOD"); |
pirottealex | 0:96bdb0d08de1 | 31 | } |
pirottealex | 0:96bdb0d08de1 | 32 | else if( statut==1) |
pirottealex | 0:96bdb0d08de1 | 33 | { |
pirottealex | 0:96bdb0d08de1 | 34 | pc.printf("Statut = 1 : WARNING"); |
pirottealex | 0:96bdb0d08de1 | 35 | } |
pirottealex | 0:96bdb0d08de1 | 36 | else if( statut==2) |
pirottealex | 0:96bdb0d08de1 | 37 | { |
pirottealex | 0:96bdb0d08de1 | 38 | pc.printf("Statut = 2 : ERROR"); |
pirottealex | 0:96bdb0d08de1 | 39 | } |
pirottealex | 0:96bdb0d08de1 | 40 | pc.printf("\n\r"); |
pirottealex | 0:96bdb0d08de1 | 41 | for(int j=0; j<=50;j++) |
pirottealex | 0:96bdb0d08de1 | 42 | { |
pirottealex | 0:96bdb0d08de1 | 43 | msg[i]=0; |
pirottealex | 0:96bdb0d08de1 | 44 | } |
pirottealex | 0:96bdb0d08de1 | 45 | i=0; |
pirottealex | 0:96bdb0d08de1 | 46 | } |
pirottealex | 0:96bdb0d08de1 | 47 | |
pirottealex | 0:96bdb0d08de1 | 48 | |
pirottealex | 0:96bdb0d08de1 | 49 | void get_info(void) |
pirottealex | 0:96bdb0d08de1 | 50 | { |
pirottealex | 0:96bdb0d08de1 | 51 | Lidar.putc(SYNC); |
pirottealex | 0:96bdb0d08de1 | 52 | Lidar.putc(GET_INFO); |
pirottealex | 0:96bdb0d08de1 | 53 | wait(0.1); |
pirottealex | 0:96bdb0d08de1 | 54 | pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",msg[BOFF],msg[BOFF+2],msg[BOFF+1]); |
pirottealex | 0:96bdb0d08de1 | 55 | for(int j=BOFF+3; j<=16+BOFF+3;j++) |
pirottealex | 0:96bdb0d08de1 | 56 | { |
pirottealex | 0:96bdb0d08de1 | 57 | pc.printf("%x",msg[j]); |
pirottealex | 0:96bdb0d08de1 | 58 | } |
pirottealex | 0:96bdb0d08de1 | 59 | pc.printf("\n\r"); |
pirottealex | 0:96bdb0d08de1 | 60 | for(int j=0; j<=50;j++) |
pirottealex | 0:96bdb0d08de1 | 61 | { |
pirottealex | 0:96bdb0d08de1 | 62 | msg[i]=0; |
pirottealex | 0:96bdb0d08de1 | 63 | } |
pirottealex | 0:96bdb0d08de1 | 64 | i=0; |
pirottealex | 0:96bdb0d08de1 | 65 | } |
pirottealex | 0:96bdb0d08de1 | 66 | |
pirottealex | 0:96bdb0d08de1 | 67 | void reception (void) |
pirottealex | 0:96bdb0d08de1 | 68 | { |
pirottealex | 0:96bdb0d08de1 | 69 | msg[i] =Lidar.getc(); |
pirottealex | 0:96bdb0d08de1 | 70 | /*if((flag==0)&&(i==6)) |
pirottealex | 0:96bdb0d08de1 | 71 | { |
pirottealex | 0:96bdb0d08de1 | 72 | 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]); |
pirottealex | 0:96bdb0d08de1 | 73 | flag=1; |
pirottealex | 0:96bdb0d08de1 | 74 | i=-1; |
pirottealex | 0:96bdb0d08de1 | 75 | for(int j=0; j<=50;j++) |
pirottealex | 0:96bdb0d08de1 | 76 | { |
pirottealex | 0:96bdb0d08de1 | 77 | msg[i]=0; |
pirottealex | 0:96bdb0d08de1 | 78 | } |
pirottealex | 0:96bdb0d08de1 | 79 | } |
pirottealex | 0:96bdb0d08de1 | 80 | //pc.printf("%d",c); |
pirottealex | 0:96bdb0d08de1 | 81 | |
pirottealex | 0:96bdb0d08de1 | 82 | if((i==4) && (flag==1)) |
pirottealex | 0:96bdb0d08de1 | 83 | { |
pirottealex | 0:96bdb0d08de1 | 84 | if(msg[1] & 0xb1) |
pirottealex | 0:96bdb0d08de1 | 85 | int qualite= msg[0]>>2; |
pirottealex | 0:96bdb0d08de1 | 86 | if(qualite>=2) |
pirottealex | 0:96bdb0d08de1 | 87 | { |
pirottealex | 0:96bdb0d08de1 | 88 | int angle=((msg[1] >> 1) + (msg[2] << 7)) / 64.0; |
pirottealex | 0:96bdb0d08de1 | 89 | int dist=((msg[3]) + ((msg[4]) << 8)) / 4.0; |
pirottealex | 0:96bdb0d08de1 | 90 | pc.printf("%d ,%d ,%d\n\r",qualite,angle,dist); |
pirottealex | 0:96bdb0d08de1 | 91 | } |
pirottealex | 0:96bdb0d08de1 | 92 | //pc.printf("%d",i); |
pirottealex | 0:96bdb0d08de1 | 93 | i=-1; |
pirottealex | 0:96bdb0d08de1 | 94 | for(int j=0; j<=50;j++) |
pirottealex | 0:96bdb0d08de1 | 95 | { |
pirottealex | 0:96bdb0d08de1 | 96 | msg[i]=0; |
pirottealex | 0:96bdb0d08de1 | 97 | } |
pirottealex | 0:96bdb0d08de1 | 98 | }*/ |
pirottealex | 0:96bdb0d08de1 | 99 | |
pirottealex | 0:96bdb0d08de1 | 100 | i++; |
pirottealex | 0:96bdb0d08de1 | 101 | |
pirottealex | 0:96bdb0d08de1 | 102 | } |