Julien VILLEMEJANE
/
ProTIS_Lidar_mbed5
Lidar RPLidar A2M8 - Testing program on L476RG
rplidar.cpp@1:cce1f1fab87f, 2021-12-12 (annotated)
- Committer:
- villemejane
- Date:
- Sun Dec 12 12:36:57 2021 +0000
- Revision:
- 1:cce1f1fab87f
- Parent:
- 0:b57a1ecda8dd
RP Lidar Testing program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
villemejane | 0:b57a1ecda8dd | 1 | #include "mbed.h" |
villemejane | 0:b57a1ecda8dd | 2 | #include "rplidar.h" |
villemejane | 0:b57a1ecda8dd | 3 | #include <cstdio> |
villemejane | 0:b57a1ecda8dd | 4 | |
villemejane | 0:b57a1ecda8dd | 5 | void print_int(const char *name, int ki){ |
villemejane | 0:b57a1ecda8dd | 6 | pc.printf("\t %s = %d\r\n", name, ki); |
villemejane | 0:b57a1ecda8dd | 7 | } |
villemejane | 0:b57a1ecda8dd | 8 | |
villemejane | 0:b57a1ecda8dd | 9 | void print_data(const char *name, char *datai, int sizedata){ |
villemejane | 0:b57a1ecda8dd | 10 | pc.printf("\t %s = ", name); |
villemejane | 0:b57a1ecda8dd | 11 | for(int i = 0; i < sizedata; i++){ |
villemejane | 0:b57a1ecda8dd | 12 | pc.printf("%x ", datai[i]); |
villemejane | 0:b57a1ecda8dd | 13 | } |
villemejane | 0:b57a1ecda8dd | 14 | pc.printf("\r\n"); |
villemejane | 0:b57a1ecda8dd | 15 | } |
villemejane | 0:b57a1ecda8dd | 16 | |
villemejane | 0:b57a1ecda8dd | 17 | void wait_s(float sec){ |
villemejane | 0:b57a1ecda8dd | 18 | wait_us(sec*1000000); |
villemejane | 0:b57a1ecda8dd | 19 | } |
villemejane | 0:b57a1ecda8dd | 20 | |
villemejane | 1:cce1f1fab87f | 21 | void findMax(int *int_data, int angle_min, int angle_max, int *value, int *indice){ |
villemejane | 0:b57a1ecda8dd | 22 | *value = 0; |
villemejane | 0:b57a1ecda8dd | 23 | *indice = 0; |
villemejane | 1:cce1f1fab87f | 24 | for(int k = angle_min; k < angle_max; k++){ |
villemejane | 0:b57a1ecda8dd | 25 | if(int_data[k] > *value){ |
villemejane | 0:b57a1ecda8dd | 26 | *value = int_data[k]; |
villemejane | 0:b57a1ecda8dd | 27 | *indice = k; |
villemejane | 0:b57a1ecda8dd | 28 | } |
villemejane | 0:b57a1ecda8dd | 29 | } |
villemejane | 0:b57a1ecda8dd | 30 | } |
villemejane | 0:b57a1ecda8dd | 31 | |
villemejane | 0:b57a1ecda8dd | 32 | void IT_lidar(void){ |
villemejane | 0:b57a1ecda8dd | 33 | char data, startt, nostartt; |
villemejane | 0:b57a1ecda8dd | 34 | debug_data = 1; |
villemejane | 0:b57a1ecda8dd | 35 | data = lidar.getc(); |
villemejane | 0:b57a1ecda8dd | 36 | |
villemejane | 0:b57a1ecda8dd | 37 | if(scan_ok){ |
villemejane | 0:b57a1ecda8dd | 38 | switch(data_scan_nb % 5){ |
villemejane | 0:b57a1ecda8dd | 39 | case 0 : |
villemejane | 0:b57a1ecda8dd | 40 | data_ok = 0; |
villemejane | 0:b57a1ecda8dd | 41 | data_ok_q = 0; |
villemejane | 0:b57a1ecda8dd | 42 | if (((data&0X03)==0X01) || ((data&0X03)==0X02)) { |
villemejane | 0:b57a1ecda8dd | 43 | trame_ok=1; |
villemejane | 0:b57a1ecda8dd | 44 | } else { |
villemejane | 0:b57a1ecda8dd | 45 | trame_ok=0; |
villemejane | 0:b57a1ecda8dd | 46 | } |
villemejane | 0:b57a1ecda8dd | 47 | ld_current.quality = data >> 2; |
villemejane | 0:b57a1ecda8dd | 48 | startt = data & 0x01; |
villemejane | 0:b57a1ecda8dd | 49 | nostartt = (data & 0x02) >> 1; |
villemejane | 0:b57a1ecda8dd | 50 | if((data & 0x01) == 0x01){ |
villemejane | 0:b57a1ecda8dd | 51 | debug_out = 1; |
villemejane | 0:b57a1ecda8dd | 52 | for(int k = 0; k < 360; k++){ |
villemejane | 0:b57a1ecda8dd | 53 | distance_scan_old[k] = distance_scan[k]; |
villemejane | 0:b57a1ecda8dd | 54 | distance_scan[k] = 0; |
villemejane | 0:b57a1ecda8dd | 55 | } |
villemejane | 0:b57a1ecda8dd | 56 | debug_out = 0; |
villemejane | 0:b57a1ecda8dd | 57 | tour_ok++; |
villemejane | 0:b57a1ecda8dd | 58 | } |
villemejane | 0:b57a1ecda8dd | 59 | if(startt == nostartt) data_scan_nb = 0; |
villemejane | 0:b57a1ecda8dd | 60 | break; |
villemejane | 0:b57a1ecda8dd | 61 | case 1 : |
villemejane | 0:b57a1ecda8dd | 62 | if((data&0x01) == 0){ |
villemejane | 0:b57a1ecda8dd | 63 | trame_ok = 0; |
villemejane | 0:b57a1ecda8dd | 64 | data_scan_nb = 0; |
villemejane | 0:b57a1ecda8dd | 65 | } |
villemejane | 0:b57a1ecda8dd | 66 | // angle_q6[6:0] / 64 and check (degre) |
villemejane | 0:b57a1ecda8dd | 67 | ld_current.angle = data >> (1 + 6); |
villemejane | 0:b57a1ecda8dd | 68 | // check ? |
villemejane | 0:b57a1ecda8dd | 69 | break; |
villemejane | 0:b57a1ecda8dd | 70 | case 2 : |
villemejane | 0:b57a1ecda8dd | 71 | // angle_q6[14:7] / 64 (degre) |
villemejane | 0:b57a1ecda8dd | 72 | ld_current.angle += data << 1; |
villemejane | 0:b57a1ecda8dd | 73 | break; |
villemejane | 0:b57a1ecda8dd | 74 | case 3 : |
villemejane | 0:b57a1ecda8dd | 75 | // distance_q2[7:0] / 4 (mm) |
villemejane | 0:b57a1ecda8dd | 76 | ld_current.distance = data >> 2; |
villemejane | 0:b57a1ecda8dd | 77 | break; |
villemejane | 0:b57a1ecda8dd | 78 | default : |
villemejane | 0:b57a1ecda8dd | 79 | // distance_q2[15:8] / 4 (mm) |
villemejane | 0:b57a1ecda8dd | 80 | ld_current.distance += data << 6; |
villemejane | 0:b57a1ecda8dd | 81 | if(trame_ok){ |
villemejane | 0:b57a1ecda8dd | 82 | distance_scan[ld_current.angle%360] = ld_current.distance; |
villemejane | 0:b57a1ecda8dd | 83 | data_ok = 1; |
villemejane | 0:b57a1ecda8dd | 84 | if(ld_current.quality > 0) data_ok_q = 1; |
villemejane | 0:b57a1ecda8dd | 85 | } |
villemejane | 0:b57a1ecda8dd | 86 | } |
villemejane | 0:b57a1ecda8dd | 87 | data_scan_nb++; |
villemejane | 0:b57a1ecda8dd | 88 | } |
villemejane | 0:b57a1ecda8dd | 89 | else{ |
villemejane | 0:b57a1ecda8dd | 90 | data_ok = 0; |
villemejane | 0:b57a1ecda8dd | 91 | data_ok_q = 0; |
villemejane | 0:b57a1ecda8dd | 92 | received_data[data_nb] = data; |
villemejane | 0:b57a1ecda8dd | 93 | data_nb++; |
villemejane | 0:b57a1ecda8dd | 94 | } |
villemejane | 0:b57a1ecda8dd | 95 | debug_data = 0; |
villemejane | 0:b57a1ecda8dd | 96 | } |
villemejane | 0:b57a1ecda8dd | 97 | |
villemejane | 0:b57a1ecda8dd | 98 | |
villemejane | 0:b57a1ecda8dd | 99 | void sendResetReq(void){ |
villemejane | 0:b57a1ecda8dd | 100 | mode = LIDAR_MODE_RESET; |
villemejane | 0:b57a1ecda8dd | 101 | char data[2] = {0xA5, 0x40}; |
villemejane | 0:b57a1ecda8dd | 102 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 103 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 104 | wait_us(10000); |
villemejane | 0:b57a1ecda8dd | 105 | } |
villemejane | 0:b57a1ecda8dd | 106 | |
villemejane | 0:b57a1ecda8dd | 107 | void getHealthLidar(void){ |
villemejane | 0:b57a1ecda8dd | 108 | stopScan(); |
villemejane | 0:b57a1ecda8dd | 109 | mode = LIDAR_MODE_HEALTH; |
villemejane | 0:b57a1ecda8dd | 110 | char data[2] = {0xA5, LIDAR_MODE_HEALTH}; |
villemejane | 0:b57a1ecda8dd | 111 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 112 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 113 | data_nb = 0; |
villemejane | 0:b57a1ecda8dd | 114 | while(data_nb != (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)){__nop();} |
villemejane | 0:b57a1ecda8dd | 115 | //print_data("Health", received_data, (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)); |
villemejane | 0:b57a1ecda8dd | 116 | if(received_data[7] == 0) pc.printf("\r\nGOOD\r\n"); |
villemejane | 0:b57a1ecda8dd | 117 | else pc.printf("\r\nBAD\r\n"); |
villemejane | 0:b57a1ecda8dd | 118 | |
villemejane | 0:b57a1ecda8dd | 119 | } |
villemejane | 0:b57a1ecda8dd | 120 | |
villemejane | 0:b57a1ecda8dd | 121 | void getInfoLidar(void){ |
villemejane | 0:b57a1ecda8dd | 122 | stopScan(); |
villemejane | 0:b57a1ecda8dd | 123 | mode = LIDAR_MODE_INFO; |
villemejane | 0:b57a1ecda8dd | 124 | char data[2] = {0xA5, LIDAR_MODE_INFO}; |
villemejane | 0:b57a1ecda8dd | 125 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 126 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 127 | data_nb = 0; |
villemejane | 0:b57a1ecda8dd | 128 | while(data_nb != (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)){__nop();} |
villemejane | 0:b57a1ecda8dd | 129 | print_data("Info", received_data, (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)); |
villemejane | 0:b57a1ecda8dd | 130 | } |
villemejane | 0:b57a1ecda8dd | 131 | |
villemejane | 0:b57a1ecda8dd | 132 | void getSampleRate(void){ |
villemejane | 0:b57a1ecda8dd | 133 | stopScan(); |
villemejane | 0:b57a1ecda8dd | 134 | mode = LIDAR_MODE_RATE; |
villemejane | 0:b57a1ecda8dd | 135 | char data[2] = {0xA5, LIDAR_MODE_RATE}; |
villemejane | 0:b57a1ecda8dd | 136 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 137 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 138 | data_nb = 0; |
villemejane | 0:b57a1ecda8dd | 139 | while(data_nb != (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)){__nop();} |
villemejane | 0:b57a1ecda8dd | 140 | print_data("Rate", received_data, (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)); |
villemejane | 0:b57a1ecda8dd | 141 | int usRate = (received_data[8] << 8) + received_data[7]; |
villemejane | 0:b57a1ecda8dd | 142 | print_int("Standard (uS) ", usRate); |
villemejane | 0:b57a1ecda8dd | 143 | usRate = (received_data[10] << 8) + received_data[9]; |
villemejane | 0:b57a1ecda8dd | 144 | print_int("Express (uS) ", usRate); |
villemejane | 0:b57a1ecda8dd | 145 | } |
villemejane | 0:b57a1ecda8dd | 146 | |
villemejane | 0:b57a1ecda8dd | 147 | void startScan(void){ |
villemejane | 0:b57a1ecda8dd | 148 | stopScan(); |
villemejane | 0:b57a1ecda8dd | 149 | mode = LIDAR_MODE_SCAN; |
villemejane | 0:b57a1ecda8dd | 150 | char data[2] = {0xA5, LIDAR_MODE_SCAN}; |
villemejane | 0:b57a1ecda8dd | 151 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 152 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 153 | data_nb = 0; |
villemejane | 0:b57a1ecda8dd | 154 | data_scan_nb = 0; |
villemejane | 0:b57a1ecda8dd | 155 | while(data_nb != (NB_BYTE_SCAN_REQ)){__nop();} |
villemejane | 0:b57a1ecda8dd | 156 | pc.printf("over"); |
villemejane | 0:b57a1ecda8dd | 157 | scan_ok = 1; |
villemejane | 0:b57a1ecda8dd | 158 | } |
villemejane | 0:b57a1ecda8dd | 159 | |
villemejane | 0:b57a1ecda8dd | 160 | void stopScan(void){ |
villemejane | 0:b57a1ecda8dd | 161 | mode = LIDAR_MODE_STOP; |
villemejane | 0:b57a1ecda8dd | 162 | scan_ok = 0; |
villemejane | 0:b57a1ecda8dd | 163 | char data[2] = {0xA5, LIDAR_MODE_STOP}; |
villemejane | 0:b57a1ecda8dd | 164 | lidar.putc(data[0]); |
villemejane | 0:b57a1ecda8dd | 165 | lidar.putc(data[1]); |
villemejane | 0:b57a1ecda8dd | 166 | } |