Julien VILLEMEJANE / Mbed OS ProTIS_Lidar_mbed5

Lidar RPLidar A2M8 - Testing program on L476RG

rplidar.cpp@0:b57a1ecda8dd, 2021-04-13 (annotated)

Committer:
villemejane
Date:
Tue Apr 13 12:42:42 2021 +0000
Revision:
0:b57a1ecda8dd
Child:
1:cce1f1fab87f
Lidar RPLidar A2M8 - Testing program on L476RG

Who changed what in which revision?

UserRevisionLine numberNew 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 0:b57a1ecda8dd 21 void findMax(int *int_data, int size_data, int *value, int *indice){
villemejane 0:b57a1ecda8dd 22 *value = 0;
villemejane 0:b57a1ecda8dd 23 *indice = 0;
villemejane 0:b57a1ecda8dd 24 for(int k = 0; k < size_data; 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 }