Julien VILLEMEJANE / Mbed OS ProTIS_Lidar_mbed6

Test for data scanning of RPLidar A2M8

rplidar.cpp@0:439469932304, 2021-03-25 (annotated)

Committer:
villemejane
Date:
Thu Mar 25 10:11:59 2021 +0000
Revision:
0:439469932304
Test for data scanning of RPLidar A2M8

Who changed what in which revision?

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