Julien VILLEMEJANE
/
ProTIS_Lidar_mbed6
Test for data scanning of RPLidar A2M8
rplidar.cpp
- Committer:
- villemejane
- Date:
- 2021-03-25
- Revision:
- 0:439469932304
File content as of revision 0:439469932304:
#include "mbed.h" #include "rplidar.h" #include <cstdio> void print_int(const char *name, int ki){ char data_to_send[64]; sprintf(data_to_send, "\t %s = %d\r\n", name, ki); pc.write(data_to_send, strlen(data_to_send)); } void print_data(const char *name, char *datai, int sizedata){ char data_to_send[64]; sprintf(data_to_send, "\t %s = ", name); pc.write(data_to_send, strlen(data_to_send)); for(int i = 0; i < sizedata; i++){ sprintf(data_to_send, "%x ", datai[i]); pc.write(data_to_send, strlen(data_to_send)); } pc.write("\r\n", strlen("\r\n")); } void wait_s(float sec){ wait_us(sec*1000000); } void findMax(int *int_data, int size_data, int *value, int *indice){ *value = 0; *indice = 0; for(int k = 0; k < size_data; k++){ if(int_data[k] > *value){ *value = int_data[k]; *indice = k; } } } void IT_lidar(void){ char data, startt, nostartt; debug_data = 1; lidar.read(&data, 1); if(scan_ok){ switch(data_scan_nb % 5){ case 0 : data_ok = 0; data_ok_q = 0; if (((data&0X03)==0X01) || ((data&0X03)==0X02)) { trame_ok=1; } else { trame_ok=0; } ld_current.quality = data >> 2; startt = data & 0x01; nostartt = (data & 0x02) >> 1; if((data & 0x01) == 0x01){ debug_out = 1; for(int k = 0; k < 360; k++){ distance_scan_old[k] = distance_scan[k]; distance_scan[k] = 0; } debug_out = 0; tour_ok++; } if(startt == nostartt) data_scan_nb = 0; break; case 1 : if((data&0x01) == 0){ trame_ok = 0; data_scan_nb = 0; } // angle_q6[6:0] / 64 and check (degre) ld_current.angle = data >> (1 + 6); // check ? break; case 2 : // angle_q6[14:7] / 64 (degre) ld_current.angle += data << 1; break; case 3 : // distance_q2[7:0] / 4 (mm) ld_current.distance = data >> 2; break; default : // distance_q2[15:8] / 4 (mm) ld_current.distance += data << 6; if(trame_ok){ distance_scan[ld_current.angle%360] = ld_current.distance; data_ok = 1; if(ld_current.quality > 0) data_ok_q = 1; } } data_scan_nb++; } else{ data_ok = 0; data_ok_q = 0; received_data[data_nb] = data; data_nb++; } debug_data = 0; } void sendResetReq(void){ mode = LIDAR_MODE_RESET; char data[2] = {0xA5, 0x40}; lidar.write(data, 2); wait_us(10000); } void getHealthLidar(void){ stopScan(); mode = LIDAR_MODE_HEALTH; char data[2] = {0xA5, LIDAR_MODE_HEALTH}; lidar.write(data, 2); data_nb = 0; while(data_nb != (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)){__nop();} //print_data("Health", received_data, (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)); if(received_data[7] == 0) pc.write("\r\nGOOD\r\n", sizeof("\r\nGOOD\r\n")); else pc.write("\r\nBAD\r\n", sizeof("\r\nBAD\r\n")); } void getInfoLidar(void){ stopScan(); mode = LIDAR_MODE_INFO; char data[2] = {0xA5, LIDAR_MODE_INFO}; lidar.write(data, 2); data_nb = 0; while(data_nb != (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)){__nop();} print_data("Info", received_data, (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)); } void getSampleRate(void){ stopScan(); mode = LIDAR_MODE_RATE; char data[2] = {0xA5, LIDAR_MODE_RATE}; lidar.write(data, 2); data_nb = 0; while(data_nb != (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)){__nop();} print_data("Rate", received_data, (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)); int usRate = (received_data[8] << 8) + received_data[7]; print_int("Standard (uS) ", usRate); usRate = (received_data[10] << 8) + received_data[9]; print_int("Express (uS) ", usRate); } void startScan(void){ stopScan(); mode = LIDAR_MODE_SCAN; char data[2] = {0xA5, LIDAR_MODE_SCAN}; lidar.write(data, 2); data_nb = 0; data_scan_nb = 0; while(data_nb != (NB_BYTE_SCAN_REQ)){__nop();} scan_ok = 1; } void stopScan(void){ mode = LIDAR_MODE_STOP; scan_ok = 0; char data[2] = {0xA5, LIDAR_MODE_STOP}; lidar.write(data, 2); }