Diff: rplidar.cpp

Revision:

0:439469932304

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar.cpp	Thu Mar 25 10:11:59 2021 +0000
@@ -0,0 +1,164 @@
+#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);
+}