Julien VILLEMEJANE
Test for data scanning of RPLidar A2M8
Revision 0:439469932304, committed 2021-03-25
- Comitter:
- villemejane
- Date:
- Thu Mar 25 10:11:59 2021 +0000
- Commit message:
- Test for data scanning of RPLidar A2M8
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.gitignore Thu Mar 25 10:11:59 2021 +0000 @@ -0,0 +1,4 @@ +.build +.mbed +projectfiles +*.py*
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CONTRIBUTING.md Thu Mar 25 10:11:59 2021 +0000 @@ -0,0 +1,5 @@ +# Contributing to Mbed OS + +Mbed OS is an open-source, device software platform for the Internet of Things. Contributions are an important part of the platform, and our goal is to make it as simple as possible to become a contributor. + +To encourage productive collaboration, as well as robust, consistent and maintainable code, we have a set of guidelines for [contributing to Mbed OS](https://os.mbed.com/docs/mbed-os/latest/contributing/index.html).
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/README.md Thu Mar 25 10:11:59 2021 +0000 @@ -0,0 +1,64 @@ + +# Blinky Mbed OS example + +The example project is part of the [Arm Mbed OS Official Examples](https://os.mbed.com/code/) and is the [getting started example for Mbed OS](https://os.mbed.com/docs/mbed-os/v5.14/quick-start/index.html). It contains an application that repeatedly blinks an LED on supported [Mbed boards](https://os.mbed.com/platforms/). + +You can build the project with all supported [Mbed OS build tools](https://os.mbed.com/docs/mbed-os/latest/tools/index.html). However, this example project specifically refers to the command-line interface tool [Arm Mbed CLI](https://github.com/ARMmbed/mbed-cli#installing-mbed-cli). +(Note: To see a rendered example you can import into the Arm Online Compiler, please see our [import quick start](https://os.mbed.com/docs/mbed-os/latest/quick-start/online-with-the-online-compiler.html#importing-the-code).) + +1. [Install Mbed CLI](https://os.mbed.com/docs/mbed-os/latest/quick-start/offline-with-mbed-cli.html). + +1. Clone this repository on your system, and change the current directory to where the project was cloned: + + ```bash + $ git clone git@github.com:armmbed/mbed-os-example-blinky && cd mbed-os-example-blinky + ``` + + Alternatively, you can download the example project with Arm Mbed CLI using the `import` subcommand: + + ```bash + $ mbed import mbed-os-example-blinky && cd mbed-os-example-blinky + ``` + + +## Application functionality + +The `main()` function is the single thread in the application. It toggles the state of a digital output connected to an LED on the board. + +## Building and running + +1. Connect a USB cable between the USB port on the board and the host computer. +2. <a name="build_cmd"></a> Run the following command to build the example project and program the microcontroller flash memory: + ```bash + $ mbed compile -m <TARGET> -t <TOOLCHAIN> --flash + ``` +The binary is located at `./BUILD/<TARGET>/<TOOLCHAIN>/mbed-os-example-blinky.bin`. + +Alternatively, you can manually copy the binary to the board, which you mount on the host computer over USB. + +Depending on the target, you can build the example project with the `GCC_ARM`, `ARM` or `IAR` toolchain. After installing Arm Mbed CLI, run the command below to determine which toolchain supports your target: + +```bash +$ mbed compile -S +``` + +## Expected output +The LED on your target turns on and off every 500 milliseconds. + + +## Troubleshooting +If you have problems, you can review the [documentation](https://os.mbed.com/docs/latest/tutorials/debugging.html) for suggestions on what could be wrong and how to fix it. + +## Related Links + +* [Mbed OS Stats API](https://os.mbed.com/docs/latest/apis/mbed-statistics.html). +* [Mbed OS Configuration](https://os.mbed.com/docs/latest/reference/configuration.html). +* [Mbed OS Serial Communication](https://os.mbed.com/docs/latest/tutorials/serial-communication.html). +* [Mbed OS bare metal](https://os.mbed.com/docs/mbed-os/latest/reference/mbed-os-bare-metal.html). +* [Mbed boards](https://os.mbed.com/platforms/). + +### License and contributions + +The software is provided under Apache-2.0 license. Contributions to this project are accepted under the same license. Please see contributing.md for more info. + +This project contains code from other projects. The original license text is included in those source files. They must comply with our license guide.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 25 10:11:59 2021 +0000
@@ -0,0 +1,88 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "rplidar.h"
+
+#define BLINKING_RATE_MS 500
+#define NB_DATA_MAX 20
+#define AFF_DATA 0
+
+char pc_debug_data[128];
+char received_data[64];
+int data_nb = 0;
+int data_scan_nb = 0;
+char mode = LIDAR_MODE_STOP;
+char scan_ok = 0;
+int distance_scan[360] = {0};
+int distance_scan_old[360] = {0};
+char tour_ok = 0;
+char trame_ok = 0;
+
+UnbufferedSerial pc(USBTX, USBRX, 115200);
+DigitalOut led(LED1);
+DigitalOut debug_data(D10);
+DigitalOut debug_tour(D9);
+DigitalOut debug_out(D7);
+DigitalOut data_ok(D5);
+DigitalOut data_ok_q(D4);
+
+UnbufferedSerial lidar(A0, A1, 115200);
+PwmOut rotation(A3);
+
+struct lidar_data ld_current;
+
+
+/** MAIN FUNCTION
+ */
+int main()
+{
+ int nb_tour = 0;
+ wait_s(3.0);
+ rotation.period(1/25000.0);
+ rotation.write(0.4);
+ wait_s(2.0);
+ pc.write("\r\nLIDAR Testing\r\n", sizeof("\r\nLIDAR Testing\r\n")+1);
+ lidar.attach(&IT_lidar);
+ wait_s(1.0);
+ pc.write("\r\nLIDAR OK\r\n", sizeof("\r\nLIDAR OK\r\n")+1);
+
+ getHealthLidar();
+ getInfoLidar();
+ getSampleRate();
+ // Start a new scan
+ startScan();
+ // Infinite Loop
+ while (true) {
+ if(trame_ok){
+ debug_tour = !debug_tour;
+ }
+
+ if(tour_ok == 4){
+ int maxDistance, maxAngle;
+ tour_ok = 0;
+ findMax(distance_scan_old, 360, &maxDistance, &maxAngle);
+ print_int("A", maxAngle);
+ }
+ /*
+ stopScan();
+ tour_ok = 0;
+ print_int("NB ", data_scan_nb);
+ // affichage données
+ if(AFF_DATA){
+ for(int k = 0; k < 360; k++){
+ if(distance_scan_old[k] != 0){
+ sprintf(pc_debug_data, "\t%d = %d", k, distance_scan_old[k]);
+ pc.write(pc_debug_data, strlen(pc_debug_data));
+ }
+ }
+ }
+ getHealthLidar();
+ wait_s(0.001);
+ startScan();
+ */
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Thu Mar 25 10:11:59 2021 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os.git/#96e19afdd196c6c99edd58fddd44e2c691cdca2f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_app.json Thu Mar 25 10:11:59 2021 +0000
@@ -0,0 +1,3 @@
+{
+ "requires" : ["bare-metal"]
+}
\ No newline at end of file
Binary file resources/official_armmbed_example_badge.png has changed
--- /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);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar.h Thu Mar 25 10:11:59 2021 +0000
@@ -0,0 +1,104 @@
+#ifndef __include_rplidar_h__
+#define __include_rplidar_h__
+
+#include "mbed.h"
+#define LIDAR_MODE_STOP 0x25
+#define LIDAR_MODE_RESET 0x40
+#define LIDAR_MODE_SCAN 0x20
+#define LIDAR_MODE_FORCE 0x21
+#define LIDAR_MODE_INFO 0x50
+#define LIDAR_MODE_HEALTH 0x52
+#define LIDAR_MODE_RATE 0x59
+
+#define NB_BYTE_INFO_REQ 7
+#define NB_BYTE_INFO_RESP 20
+#define NB_BYTE_RATE_REQ 7
+#define NB_BYTE_RATE_RESP 4
+#define NB_BYTE_HEALTH_REQ 7
+#define NB_BYTE_HEALTH_RESP 3
+#define NB_BYTE_FORCE_REQ 7
+#define NB_BYTE_FORCE_RESP 5
+#define NB_BYTE_SCAN_REQ 7
+
+extern char pc_debug_data[128];
+
+extern UnbufferedSerial lidar;
+extern UnbufferedSerial pc;
+extern DigitalOut debug_data;
+extern DigitalOut debug_out;
+extern DigitalOut data_ok;
+extern DigitalOut data_ok_q;
+extern int data_nb;
+extern int data_scan_nb;
+extern char received_data[];
+extern char mode;
+extern char scan_ok;
+extern int distance_scan[];
+extern int distance_scan_old[];
+extern char tour_ok;
+extern char trame_ok;
+extern struct lidar_data ld_current;
+
+/* Data Structure of lidar */
+struct lidar_data{
+ int quality;
+ int angle;
+ int distance;
+};
+
+/*********************************************************************** GENERAL FUNCTIONS */
+
+/** Print int value and its name
+ */
+void print_int(const char *name, int ki);
+/** Print data from serial communication
+ */
+void print_data(const char *name, char *datai, int sizedata);
+/** Wait seconds
+ */
+void wait_s(float sec);
+
+/** Find max in an integer array
+ */
+void findMax(int *int_data, int size, int *value, int *indice);
+
+/************************************************************************* LIDAR FUNCTIONS */
+
+/** IT_lidar
+ interrupt function on serial receiving
+ */
+void IT_lidar(void);
+
+/** Reset request
+ send command to core reset of the lidar
+ this action took 2ms
+ */
+void sendResetReq(void);
+
+/** Health request
+ get device health information
+ */
+void getHealthLidar(void);
+
+/** Info request
+ get device information
+ model / firmware _ LSB / MSB / Hardware / SerialNumber (15 octets)
+ */
+void getInfoLidar(void);
+
+/** Sample Rate
+ get sample rate
+ */
+void getSampleRate(void);
+
+/** Start Scan
+ start standard scan
+ */
+void startScan(void);
+
+/** Stop Scan
+ stop standard scan
+ */
+void stopScan(void);
+
+#endif /* #ifndef __include_rplidar_h__ */
\ No newline at end of file