rplidar.cpp

00001 #include "mbed.h"
00002 #include "rplidar.h"
00003 #include <cstdio>
00004 
00005 void print_int(const char *name, int ki){
00006     char data_to_send[64];
00007     sprintf(data_to_send, "\t %s = %d\r\n", name, ki);
00008     pc.write(data_to_send, strlen(data_to_send));
00009 }
00010 
00011 void print_data(const char *name, char *datai, int sizedata){
00012     char data_to_send[64];
00013     sprintf(data_to_send, "\t %s = ", name);
00014     pc.write(data_to_send, strlen(data_to_send));
00015     for(int i = 0; i < sizedata; i++){
00016         sprintf(data_to_send, "%x ", datai[i]);
00017         pc.write(data_to_send, strlen(data_to_send));
00018     }
00019     pc.write("\r\n", strlen("\r\n"));
00020 }
00021 
00022 void wait_s(float sec){
00023     wait_us(sec*1000000);
00024 }
00025 
00026 void findMax(int *int_data, int size_data, int *value, int *indice){
00027     *value = 0;
00028     *indice = 0;
00029     for(int k = 0; k < size_data; k++){
00030         if(int_data[k] > *value){
00031             *value = int_data[k];
00032             *indice = k;
00033         }
00034     }
00035 }
00036 
00037 void IT_lidar(void){
00038     char data, startt, nostartt;
00039     debug_data = 1;
00040     lidar.read(&data, 1);
00041 
00042     if(scan_ok){
00043         switch(data_scan_nb % 5){
00044             case 0 :
00045                 data_ok = 0;
00046                 data_ok_q = 0;
00047                 if (((data&0X03)==0X01) || ((data&0X03)==0X02)) { 
00048                     trame_ok=1;
00049                 } else {
00050                     trame_ok=0;
00051                 }
00052                 ld_current.quality = data >> 2;
00053                 startt = data & 0x01;
00054                 nostartt = (data & 0x02) >> 1;
00055                 if((data & 0x01) == 0x01){
00056                     debug_out = 1;
00057                     for(int k = 0; k < 360; k++){
00058                         distance_scan_old[k] = distance_scan[k];
00059                         distance_scan[k] = 0;
00060                     }
00061                     debug_out = 0;
00062                     tour_ok++;
00063                 }   
00064                 if(startt == nostartt)      data_scan_nb = 0;
00065                 break;            
00066             case 1 :
00067                 if((data&0x01) == 0){
00068                     trame_ok = 0;
00069                     data_scan_nb = 0;
00070                 }
00071                 // angle_q6[6:0] / 64 and check (degre)
00072                 ld_current.angle = data >> (1 + 6);
00073                 // check ?
00074                 break;            
00075             case 2 :
00076                 // angle_q6[14:7] / 64 (degre)
00077                 ld_current.angle += data << 1;
00078                 break;            
00079             case 3 :
00080                 // distance_q2[7:0] / 4 (mm)
00081                 ld_current.distance = data >> 2;
00082                 break;
00083             default :
00084                 // distance_q2[15:8] / 4 (mm)
00085                 ld_current.distance += data << 6;
00086                 if(trame_ok){
00087                     distance_scan[ld_current.angle%360] = ld_current.distance;
00088                     data_ok = 1;
00089                     if(ld_current.quality > 0) data_ok_q = 1;
00090                 }         
00091         }
00092         data_scan_nb++;
00093     }
00094     else{
00095         data_ok = 0;
00096         data_ok_q = 0;
00097         received_data[data_nb] = data;
00098         data_nb++;
00099     }
00100     debug_data = 0;
00101 }
00102 
00103 
00104 void sendResetReq(void){
00105     mode = LIDAR_MODE_RESET;
00106     char data[2] = {0xA5, 0x40};
00107     lidar.write(data, 2);
00108     wait_us(10000);
00109 }
00110 
00111 void getHealthLidar(void){
00112     stopScan();
00113     mode = LIDAR_MODE_HEALTH;
00114     char data[2] = {0xA5, LIDAR_MODE_HEALTH};
00115     lidar.write(data, 2);
00116     data_nb = 0;
00117     while(data_nb != (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP)){__nop();}
00118     //print_data("Health", received_data, (NB_BYTE_HEALTH_REQ + NB_BYTE_HEALTH_RESP));
00119     if(received_data[7] == 0)   pc.write("\r\nGOOD\r\n", sizeof("\r\nGOOD\r\n"));
00120     else   pc.write("\r\nBAD\r\n", sizeof("\r\nBAD\r\n"));
00121 
00122 }
00123 
00124 void getInfoLidar(void){
00125     stopScan();
00126     mode = LIDAR_MODE_INFO;
00127     char data[2] = {0xA5, LIDAR_MODE_INFO};
00128     lidar.write(data, 2);
00129     data_nb = 0;
00130     while(data_nb != (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP)){__nop();}
00131     print_data("Info", received_data, (NB_BYTE_INFO_REQ + NB_BYTE_INFO_RESP));
00132 }
00133 
00134 void getSampleRate(void){
00135     stopScan();
00136     mode = LIDAR_MODE_RATE;
00137     char data[2] = {0xA5, LIDAR_MODE_RATE};
00138     lidar.write(data, 2);
00139     data_nb = 0;
00140     while(data_nb != (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP)){__nop();}
00141     print_data("Rate", received_data, (NB_BYTE_RATE_REQ + NB_BYTE_RATE_RESP));
00142     int usRate = (received_data[8] << 8) + received_data[7];
00143     print_int("Standard (uS) ", usRate);
00144     usRate = (received_data[10] << 8) + received_data[9];
00145     print_int("Express (uS) ", usRate);
00146 }
00147 
00148 void startScan(void){
00149     stopScan();
00150     mode = LIDAR_MODE_SCAN;
00151     char data[2] = {0xA5, LIDAR_MODE_SCAN};
00152     lidar.write(data, 2);
00153     data_nb = 0;
00154     data_scan_nb = 0;
00155     while(data_nb != (NB_BYTE_SCAN_REQ)){__nop();}
00156     scan_ok = 1;
00157 }
00158 
00159 void stopScan(void){
00160     mode = LIDAR_MODE_STOP;
00161     scan_ok = 0;
00162     char data[2] = {0xA5, LIDAR_MODE_STOP};
00163     lidar.write(data, 2);
00164 }