IOGS_France / rplidar

RP Lidar A2M8

rplidar.cpp@0:9803ade33ac3, 2021-12-14 (annotated)

Committer:
villemejane
Date:
Tue Dec 14 15:01:18 2021 +0000
Revision:
0:9803ade33ac3
RP Lidar A2M8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
villemejane 0:9803ade33ac3 1 /****************************************************************************/
villemejane 0:9803ade33ac3 2 /* LIDAR RPLidar A2M8 module library */
villemejane 0:9803ade33ac3 3 /****************************************************************************/
villemejane 0:9803ade33ac3 4 /* LEnsE / Julien VILLEMEJANE / Institut d'Optique Graduate School */
villemejane 0:9803ade33ac3 5 /****************************************************************************/
villemejane 0:9803ade33ac3 6 /* Library - rplidar.cpp file */
villemejane 0:9803ade33ac3 7 /****************************************************************************/
villemejane 0:9803ade33ac3 8 /* Tested on Nucleo-L476RG / 4th nov 2021 */
villemejane 0:9803ade33ac3 9 /****************************************************************************/
villemejane 0:9803ade33ac3 10
villemejane 0:9803ade33ac3 11 #include "mbed.h"
villemejane 0:9803ade33ac3 12 #include "rplidar.h"
villemejane 0:9803ade33ac3 13 #include <cstdio>
villemejane 0:9803ade33ac3 14
villemejane 0:9803ade33ac3 15 #define BLINKING_RATE_MS 500
villemejane 0:9803ade33ac3 16 #define NB_DATA_MAX 20
villemejane 0:9803ade33ac3 17 #define AFF_DATA 0
villemejane 0:9803ade33ac3 18 // Lidar
villemejane 0:9803ade33ac3 19 Serial lidar(PA_0, PA_1);
villemejane 0:9803ade33ac3 20 PwmOut lidar_ct(PB_9);
villemejane 0:9803ade33ac3 21
villemejane 0:9803ade33ac3 22
villemejane 0:9803ade33ac3 23 char pc_debug_data[128];
villemejane 0:9803ade33ac3 24 char received_data[64];
villemejane 0:9803ade33ac3 25 int data_nb = 0;
villemejane 0:9803ade33ac3 26 int data_scan_nb = 0;
villemejane 0:9803ade33ac3 27 char mode = LIDAR_MODE_STOP;
villemejane 0:9803ade33ac3 28 char scan_ok = 0;
villemejane 0:9803ade33ac3 29 int distance_scan[360] = {0};
villemejane 0:9803ade33ac3 30 int distance_scan_old[360] = {0};
villemejane 0:9803ade33ac3 31 char tour_ok = 0;
villemejane 0:9803ade33ac3 32 char trame_ok = 0;
villemejane 0:9803ade33ac3 33
villemejane 0:9803ade33ac3 34 struct lidar_data ld_current;
villemejane 0:9803ade33ac3 35
villemejane 0:9803ade33ac3 36
villemejane 0:9803ade33ac3 37
villemejane 0:9803ade33ac3 38 // Fonction d'initialisation du Lidar
villemejane 0:9803ade33ac3 39 void initLidar(void){
villemejane 0:9803ade33ac3 40 lidar.baud(115200);
villemejane 0:9803ade33ac3 41 wait_ms(2000);
villemejane 0:9803ade33ac3 42 lidar_ct.period(1/25000.0);
villemejane 0:9803ade33ac3 43 lidar_ct.write(0.4);
villemejane 0:9803ade33ac3 44 wait_ms(2000);
villemejane 0:9803ade33ac3 45 debug_pc.printf("\r\nLIDAR Testing\r\n");
villemejane 0:9803ade33ac3 46 lidar.attach(&IT_lidar);
villemejane 0:9803ade33ac3 47 wait_ms(500);
villemejane 0:9803ade33ac3 48 debug_pc.printf("\r\nLIDAR OK\r\n");
villemejane 0:9803ade33ac3 49
villemejane 0:9803ade33ac3 50 getHealthLidar();
villemejane 0:9803ade33ac3 51 getInfoLidar();
villemejane 0:9803ade33ac3 52 getSampleRate();
villemejane 0:9803ade33ac3 53 }
villemejane 0:9803ade33ac3 54
villemejane 0:9803ade33ac3 55 // Fonction de test du Lidar
villemejane 0:9803ade33ac3 56 void testLidar(){
villemejane 0:9803ade33ac3 57 if(tour_ok == 6){
villemejane 0:9803ade33ac3 58 int maxDistance, maxAngle;
villemejane 0:9803ade33ac3 59 tour_ok = 0;
villemejane 0:9803ade33ac3 60 findMax(distance_scan_old, 0, 360, &maxDistance, &maxAngle);
villemejane 0:9803ade33ac3 61 print_int("A", maxAngle);
villemejane 0:9803ade33ac3 62 }
villemejane 0:9803ade33ac3 63 }
villemejane 0:9803ade33ac3 64
villemejane 0:9803ade33ac3 65 void print_int(const char *name, int ki){
villemejane 0:9803ade33ac3 66 debug_pc.printf("\t %s = %d\r\n", name, ki);
villemejane 0:9803ade33ac3 67 }
villemejane 0:9803ade33ac3 68
villemejane 0:9803ade33ac3 69 void print_data(const char *name, char *datai, int sizedata){
villemejane 0:9803ade33ac3 70 debug_pc.printf("\t %s = ", name);
villemejane 0:9803ade33ac3 71 for(int i = 0; i < sizedata; i++){
villemejane 0:9803ade33ac3 72 debug_pc.printf("%x ", datai[i]);
villemejane 0:9803ade33ac3 73 }
villemejane 0:9803ade33ac3 74 debug_pc.printf("\r\n");
villemejane 0:9803ade33ac3 75 }
villemejane 0:9803ade33ac3 76
villemejane 0:9803ade33ac3 77 void wait_s(float sec){
villemejane 0:9803ade33ac3 78 wait_us(sec*1000000);
villemejane 0:9803ade33ac3 79 }
villemejane 0:9803ade33ac3 80
villemejane 0:9803ade33ac3 81 void findMax(int *int_data, int angle_min, int angle_max, int *value, int *indice){
villemejane 0:9803ade33ac3 82 *value = 0;
villemejane 0:9803ade33ac3 83 *indice = 0;
villemejane 0:9803ade33ac3 84 for(int k = angle_min; k < angle_max; k++){
villemejane 0:9803ade33ac3 85 if(int_data[k] > *value){
villemejane 0:9803ade33ac3 86 *value = int_data[k];
villemejane 0:9803ade33ac3 87 *indice = k;
villemejane 0:9803ade33ac3 88 }
villemejane 0:9803ade33ac3 89 }
villemejane 0:9803ade33ac3 90 }
villemejane 0:9803ade33ac3 91
villemejane 0:9803ade33ac3 92 void IT_lidar(void){
villemejane 0:9803ade33ac3 93 char data, startt, nostartt;
villemejane 0:9803ade33ac3 94 data = lidar.getc();
villemejane 0:9803ade33ac3 95
villemejane 0:9803ade33ac3 96 if(scan_ok){
villemejane 0:9803ade33ac3 97 switch(data_scan_nb % 5){
villemejane 0:9803ade33ac3 98 case 0 :
villemejane 0:9803ade33ac3 99 if (((data&0X03)==0X01) || ((data&0X03)==0X02)) {
villemejane 0:9803ade33ac3 100 trame_ok=1;
villemejane 0:9803ade33ac3 101 } else {
villemejane 0:9803ade33ac3 102 trame_ok=0;
villemejane 0:9803ade33ac3 103 }
villemejane 0:9803ade33ac3 104 ld_current.quality = data >> 2;
villemejane 0:9803ade33ac3 105 startt = data & 0x01;
villemejane 0:9803ade33ac3 106 nostartt = (data & 0x02) >> 1;
villemejane 0:9803ade33ac3 107 if((data & 0x01) == 0x01){
villemejane 0:9803ade33ac3 108 for(int k = 0; k < 360; k++){
villemejane 0:9803ade33ac3 109 distance_scan_old[k] = distance_scan[k];
villemejane 0:9803ade33ac3 110 distance_scan[k] = 0;
villemejane 0:9803ade33ac3 111 }
villemejane 0:9803ade33ac3 112 tour_ok++;
villemejane 0:9803ade33ac3 113 }
villemejane 0:9803ade33ac3 114 if(startt == nostartt) data_scan_nb = 0;
villemejane 0:9803ade33ac3 115 break;
villemejane 0:9803ade33ac3 116 case 1 :
villemejane 0:9803ade33ac3 117 if((data&0x01) == 0){
villemejane 0:9803ade33ac3 118 trame_ok = 0;
villemejane 0:9803ade33ac3 119 data_scan_nb = 0;
villemejane 0:9803ade33ac3 120 }
villemejane 0:9803ade33ac3 121 // angle_q6[6:0] / 64 and check (degre)
villemejane 0:9803ade33ac3 122 ld_current.angle = data >> (1 + 6);
villemejane 0:9803ade33ac3 123 // check ?
villemejane 0:9803ade33ac3 124 break;
villemejane 0:9803ade33ac3 125 case 2 :
villemejane 0:9803ade33ac3 126 // angle_q6[14:7] / 64 (degre)
villemejane 0:9803ade33ac3 127 ld_current.angle += data << 1;
villemejane 0:9803ade33ac3 128 break;
villemejane 0:9803ade33ac3 129 case 3 :
villemejane 0:9803ade33ac3 130 // distance_q2[7:0] / 4 (mm)
villemejane 0:9803ade33ac3 131 ld_current.distance = data >> 2;
villemejane 0:9803ade33ac3 132 break;
villemejane 0:9803ade33ac3 133 default :
villemejane 0:9803ade33ac3 134 // distance_q2[15:8] / 4 (mm)
villemejane 0:9803ade33ac3 135 ld_current.distance += data << 6;
villemejane 0:9803ade33ac3 136 if(trame_ok){
villemejane 0:9803ade33ac3 137 distance_scan[ld_current.angle%360] = ld_current.distance;
villemejane 0:9803ade33ac3 138 }
villemejane 0:9803ade33ac3 139 }
villemejane 0:9803ade33ac3 140 data_scan_nb++;
villemejane 0:9803ade33ac3 141 }
villemejane 0:9803ade33ac3 142 else{
villemejane 0:9803ade33ac3 143 received_data[data_nb] = data;
villemejane 0:9803ade33ac3 144 data_nb++;
villemejane 0:9803ade33ac3 145 }
villemejane 0:9803ade33ac3 146 }
villemejane 0:9803ade33ac3 147
villemejane 0:9803ade33ac3 148
villemejane 0:9803ade33ac3 149 void sendResetReq(void){
villemejane 0:9803ade33ac3 150 mode = LIDAR_MODE_RESET;
villemejane 0:9803ade33ac3 151 char data[2] = {0xA5, 0x40};
villemejane 0:9803ade33ac3 152 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 153 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 154 wait_us(10000);
villemejane 0:9803ade33ac3 155 }
villemejane 0:9803ade33ac3 156
villemejane 0:9803ade33ac3 157 void getHealthLidar(void){
villemejane 0:9803ade33ac3 158 stopScan();
villemejane 0:9803ade33ac3 159 mode = LIDAR_MODE_HEALTH;
villemejane 0:9803ade33ac3 160 char data[2] = {0xA5, LIDAR_MODE_HEALTH};
villemejane 0:9803ade33ac3 161 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 162 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 163 data_nb = 0;
villemejane 0:9803ade33ac3 164 while(data_nb != (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)){__nop();}
villemejane 0:9803ade33ac3 165 //print_data("Health", received_data, (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP));
villemejane 0:9803ade33ac3 166 if(received_data[7] == 0) debug_pc.printf("\r\nGOOD\r\n");
villemejane 0:9803ade33ac3 167 else debug_pc.printf("\r\nBAD\r\n");
villemejane 0:9803ade33ac3 168
villemejane 0:9803ade33ac3 169 }
villemejane 0:9803ade33ac3 170
villemejane 0:9803ade33ac3 171 void getInfoLidar(void){
villemejane 0:9803ade33ac3 172 stopScan();
villemejane 0:9803ade33ac3 173 mode = LIDAR_MODE_INFO;
villemejane 0:9803ade33ac3 174 char data[2] = {0xA5, LIDAR_MODE_INFO};
villemejane 0:9803ade33ac3 175 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 176 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 177 data_nb = 0;
villemejane 0:9803ade33ac3 178 while(data_nb != (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)){__nop();}
villemejane 0:9803ade33ac3 179 print_data("Info", received_data, (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP));
villemejane 0:9803ade33ac3 180 }
villemejane 0:9803ade33ac3 181
villemejane 0:9803ade33ac3 182 void getSampleRate(void){
villemejane 0:9803ade33ac3 183 stopScan();
villemejane 0:9803ade33ac3 184 mode = LIDAR_MODE_RATE;
villemejane 0:9803ade33ac3 185 char data[2] = {0xA5, LIDAR_MODE_RATE};
villemejane 0:9803ade33ac3 186 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 187 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 188 data_nb = 0;
villemejane 0:9803ade33ac3 189 while(data_nb != (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)){__nop();}
villemejane 0:9803ade33ac3 190 print_data("Rate", received_data, (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP));
villemejane 0:9803ade33ac3 191 int usRate = (received_data[8] << 8) + received_data[7];
villemejane 0:9803ade33ac3 192 print_int("Standard (uS) ", usRate);
villemejane 0:9803ade33ac3 193 usRate = (received_data[10] << 8) + received_data[9];
villemejane 0:9803ade33ac3 194 print_int("Express (uS) ", usRate);
villemejane 0:9803ade33ac3 195 }
villemejane 0:9803ade33ac3 196
villemejane 0:9803ade33ac3 197 void startScan(void){
villemejane 0:9803ade33ac3 198 stopScan();
villemejane 0:9803ade33ac3 199 mode = LIDAR_MODE_SCAN;
villemejane 0:9803ade33ac3 200 char data[2] = {0xA5, LIDAR_MODE_SCAN};
villemejane 0:9803ade33ac3 201 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 202 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 203 data_nb = 0;
villemejane 0:9803ade33ac3 204 data_scan_nb = 0;
villemejane 0:9803ade33ac3 205 while(data_nb != (NB_BYTE_SCAN_REQ)){__nop();}
villemejane 0:9803ade33ac3 206 debug_pc.printf("over");
villemejane 0:9803ade33ac3 207 scan_ok = 1;
villemejane 0:9803ade33ac3 208 }
villemejane 0:9803ade33ac3 209
villemejane 0:9803ade33ac3 210 void stopScan(void){
villemejane 0:9803ade33ac3 211 mode = LIDAR_MODE_STOP;
villemejane 0:9803ade33ac3 212 scan_ok = 0;
villemejane 0:9803ade33ac3 213 char data[2] = {0xA5, LIDAR_MODE_STOP};
villemejane 0:9803ade33ac3 214 lidar.putc(data[0]);
villemejane 0:9803ade33ac3 215 lidar.putc(data[1]);
villemejane 0:9803ade33ac3 216 }