diff --git a/CHANGELOG.rst b/CHANGELOG.rst index dafad8a7..a6af961a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,11 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package rplidar_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.10.0 (2021-10-27) +------------------ +* Update RPLIDAR SDK to 2.0.0 +* [new feature] 1.redesign the skelton of the sdk. 2.support Rplidar S2 +* Contributors: tony,WubinXia 1.10.0 (2019-02-22) ------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt index a5f22489..50483f73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 2.8.3) project(rplidar_ros) set(RPLIDAR_SDK_PATH "./sdk/") +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") FILE(GLOB RPLIDAR_SDK_SRC "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp" diff --git a/README.md b/README.md index 12d2a236..da9cec91 100644 --- a/README.md +++ b/README.md @@ -27,8 +27,10 @@ I. Run rplidar node and view in the rviz roslaunch rplidar_ros view_rplidar.launch (for RPLIDAR A1/A2) , roslaunch rplidar_ros view_rplidar_a3.launch (for RPLIDAR A3) -or +, roslaunch rplidar_ros view_rplidar_s1.launch (for RPLIDAR S1) +or +roslaunch rplidar_ros view_rplidar_s2.launch (for RPLIDAR S2) You should see rplidar's scan result in the rviz. @@ -37,14 +39,16 @@ II. Run rplidar node and view using test application roslaunch rplidar_ros rplidar.launch (for RPLIDAR A1/A2) , roslaunch rplidar_ros rplidar_a3.launch (for RPLIDAR A3) -or +, roslaunch rplidar_ros rplidar_s1.launch (for RPLIDAR S1) +or +roslaunch rplidar_ros rplidar_s2.launch (for RPLIDAR S2) rosrun rplidar_ros rplidarNodeClient You should see rplidar's scan result in the console -Notice: the different is serial_baudrate between A1/A2 and A3/S1 +Notice: the different is serial_baudrate between A1/A2<115200> , A3/S1<256000> and S2<1000000> RPLidar frame ===================================================================== diff --git a/launch/rplidar_s2.launch b/launch/rplidar_s2.launch new file mode 100644 index 00000000..e998fc21 --- /dev/null +++ b/launch/rplidar_s2.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/rplidar_t1.launch b/launch/rplidar_t1.launch new file mode 100644 index 00000000..31d22e2c --- /dev/null +++ b/launch/rplidar_t1.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/view_rplidar_s2.launch b/launch/view_rplidar_s2.launch new file mode 100644 index 00000000..65bf1c86 --- /dev/null +++ b/launch/view_rplidar_s2.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/launch/view_rplidar_t1.launch b/launch/view_rplidar_t1.launch new file mode 100644 index 00000000..e07878ca --- /dev/null +++ b/launch/view_rplidar_t1.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/sdk/Makefile b/sdk/Makefile new file mode 100644 index 00000000..81215d9a --- /dev/null +++ b/sdk/Makefile @@ -0,0 +1,67 @@ +#/* +# * RPLIDAR SDK +# * +# * Copyright (c) 2009 - 2014 RoboPeak Team +# * http://www.robopeak.com +# * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. +# * http://www.slamtec.com +# * +# */ +#/* +# * Redistribution and use in source and binary forms, with or without +# * modification, are permitted provided that the following conditions are met: +# * +# * 1. Redistributions of source code must retain the above copyright notice, +# * this list of conditions and the following disclaimer. +# * +# * 2. Redistributions in binary form must reproduce the above copyright notice, +# * this list of conditions and the following disclaimer in the documentation +# * and/or other materials provided with the distribution. +# * +# * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +# * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +# * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +# * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +# * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +# * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +# * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, +# * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# * +# */ +# +HOME_TREE := ../ + +MODULE_NAME := $(notdir $(CURDIR)) + +include $(HOME_TREE)/mak_def.inc + +CXXSRC += src/sl_lidar_driver.cpp \ + src/hal/thread.cpp\ + src/sl_crc.cpp\ + src/sl_serial_channel.cpp\ + src/sl_tcp_channel.cpp\ + src/sl_udp_channel.cpp + +C_INCLUDES += -I$(CURDIR)/include -I$(CURDIR)/src + +ifeq ($(BUILD_TARGET_PLATFORM),Linux) +CXXSRC += src/arch/linux/net_serial.cpp \ + src/arch/linux/net_socket.cpp \ + src/arch/linux/timer.cpp +endif + + +ifeq ($(BUILD_TARGET_PLATFORM),Darwin) +CXXSRC += src/arch/macOS/net_serial.cpp \ + src/arch/macOS/net_socket.cpp \ + src/arch/macOS/timer.cpp +endif + +all: build_sdk + +include $(HOME_TREE)/mak_common.inc + +clean: clean_sdk diff --git a/sdk/README.txt b/sdk/README.txt index b05d81fd..42a4f9d4 100644 --- a/sdk/README.txt +++ b/sdk/README.txt @@ -1,5 +1,5 @@ Copyright (c) 2009 - 2014 RoboPeak Team -Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. +Copyright (c) 2014 - 2021 Shanghai Slamtec Co., Ltd. All rights reserved. Redistribution and use in source and binary forms, with or without @@ -29,6 +29,6 @@ This folder contains RPLIDAR SDK source code which is provided by RoboPeak. RoboPeak Website: http://www.robopeak.com SlamTec HomePage: http://www.slamtec.com -RPLIDAR_SDK_VERSION: 1.10.0 +RPLIDAR_SDK_VERSION: 2.0.0 Note: The SDK version may not up-to-date. rplidar product: http://www.slamtec.com/en/Lidar diff --git a/sdk/include/rplidar.h b/sdk/include/rplidar.h index 3e545a81..2f7de3d7 100644 --- a/sdk/include/rplidar.h +++ b/sdk/include/rplidar.h @@ -41,4 +41,4 @@ #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.12.0" +#define SLAMTEC_LIDAR_SDK_VERSION SL_LIDAR_SDK_VERSION diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index 89f1aec8..9922bac3 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014-2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,35 +33,40 @@ */ #pragma once - +#include "sl_lidar_cmd.h" #include "rplidar_protocol.h" // Commands //----------------------------------------- +#define RPLIDAR_AUTOBAUD_MAGICBYTE SL_LIDAR_AUTOBAUD_MAGICBYTE + // Commands without payload and response -#define RPLIDAR_CMD_STOP 0x25 -#define RPLIDAR_CMD_SCAN 0x20 -#define RPLIDAR_CMD_FORCE_SCAN 0x21 -#define RPLIDAR_CMD_RESET 0x40 +#define RPLIDAR_CMD_STOP SL_LIDAR_CMD_STOP +#define RPLIDAR_CMD_SCAN SL_LIDAR_CMD_SCAN +#define RPLIDAR_CMD_FORCE_SCAN SL_LIDAR_CMD_FORCE_SCAN +#define RPLIDAR_CMD_RESET SL_LIDAR_CMD_RESET // Commands without payload but have response -#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 -#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 +#define RPLIDAR_CMD_GET_DEVICE_INFO SL_LIDAR_CMD_GET_DEVICE_INFO +#define RPLIDAR_CMD_GET_DEVICE_HEALTH SL_LIDAR_CMD_GET_DEVICE_HEALTH + +#define RPLIDAR_CMD_GET_SAMPLERATE SL_LIDAR_CMD_GET_SAMPLERATE //added in fw 1.17 -#define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 +#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL -#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 +// Commands with payload but no response +#define RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM //added in fw 1.30 // Commands with payload and have response -#define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 -#define RPLIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 -#define RPLIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 -#define RPLIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 +#define RPLIDAR_CMD_EXPRESS_SCAN SL_LIDAR_CMD_EXPRESS_SCAN //added in fw 1.17 +#define RPLIDAR_CMD_HQ_SCAN SL_LIDAR_CMD_HQ_SCAN //added in fw 1.24 +#define RPLIDAR_CMD_GET_LIDAR_CONF SL_LIDAR_CMD_GET_LIDAR_CONF //added in fw 1.24 +#define RPLIDAR_CMD_SET_LIDAR_CONF SL_LIDAR_CMD_SET_LIDAR_CONF //added in fw 1.24 //add for A2 to set RPLIDAR motor pwm when using accessory board -#define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0 -#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF +#define RPLIDAR_CMD_SET_MOTOR_PWM SL_LIDAR_CMD_SET_MOTOR_PWM +#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG SL_LIDAR_CMD_GET_ACC_BOARD_FLAG #if defined(_WIN32) #pragma pack(1) @@ -70,231 +75,138 @@ // Payloads // ------------------------------------------ -#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 -#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail +#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL +#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE // won't been supported but keep to prevent build fail //for express working flag(extending express scan protocol) -#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 -#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 +#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST +#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION //for ultra express working flag -#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 -#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY #define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0) #define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1) #define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2) -typedef struct _rplidar_payload_express_scan_t { - _u8 working_mode; - _u16 working_flags; - _u16 param; -} __attribute__((packed)) rplidar_payload_express_scan_t; - -typedef struct _rplidar_payload_hq_scan_t { - _u8 flag; - _u8 reserved[32]; -} __attribute__((packed)) rplidar_payload_hq_scan_t; - -typedef struct _rplidar_payload_get_scan_conf_t { - _u32 type; - _u8 reserved[32]; -} __attribute__((packed)) rplidar_payload_get_scan_conf_t; -#define MAX_MOTOR_PWM 1023 -#define DEFAULT_MOTOR_PWM 660 -typedef struct _rplidar_payload_motor_pwm_t { - _u16 pwm_value; -} __attribute__((packed)) rplidar_payload_motor_pwm_t; - -typedef struct _rplidar_payload_acc_board_flag_t { - _u32 reserved; -} __attribute__((packed)) rplidar_payload_acc_board_flag_t; - -typedef struct _rplidar_payload_hq_spd_ctrl_t { - _u16 rpm; -} __attribute__((packed)) rplidar_payload_hq_spd_ctrl_t; +typedef sl_lidar_payload_express_scan_t rplidar_payload_express_scan_t; +typedef sl_lidar_payload_hq_scan_t rplidar_payload_hq_scan_t; +typedef sl_lidar_payload_get_scan_conf_t rplidar_payload_get_scan_conf_t; +typedef sl_lidar_payload_motor_pwm_t rplidar_payload_motor_pwm_t; +typedef sl_lidar_payload_acc_board_flag_t rplidar_payload_acc_board_flag_t; +typedef sl_lidar_payload_set_scan_conf_t rplidar_payload_set_scan_conf_t; +typedef sl_lidar_payload_new_bps_confirmation_t rplidar_payload_new_bps_confirmation_t; // Response // ------------------------------------------ -#define RPLIDAR_ANS_TYPE_DEVINFO 0x4 -#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 - -#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 +#define RPLIDAR_ANS_TYPE_DEVINFO SL_LIDAR_ANS_TYPE_DEVINFO +#define RPLIDAR_ANS_TYPE_DEVHEALTH SL_LIDAR_ANS_TYPE_DEVHEALTH +#define RPLIDAR_ANS_TYPE_MEASUREMENT SL_LIDAR_ANS_TYPE_MEASUREMENT // Added in FW ver 1.17 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 - - +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED +#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ // Added in FW ver 1.17 -#define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15 +#define RPLIDAR_ANS_TYPE_SAMPLE_RATE SL_LIDAR_ANS_TYPE_SAMPLE_RATE //added in FW ver 1.23alpha -#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA //added in FW ver 1.24 -#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 -#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 -#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF +#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED +#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG -#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) -typedef struct _rplidar_response_acc_board_flag_t { - _u32 support_flag; -} __attribute__((packed)) rplidar_response_acc_board_flag_t; +#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK +typedef sl_lidar_response_acc_board_flag_t rplidar_response_acc_board_flag_t; -#define RPLIDAR_STATUS_OK 0x0 -#define RPLIDAR_STATUS_WARNING 0x1 -#define RPLIDAR_STATUS_ERROR 0x2 -#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) -#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 +#define RPLIDAR_STATUS_OK SL_LIDAR_STATUS_OK +#define RPLIDAR_STATUS_WARNING SL_LIDAR_STATUS_WARNING +#define RPLIDAR_STATUS_ERROR SL_LIDAR_STATUS_ERROR -#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) +#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT +#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT SL_LIDAR_RESP_HQ_FLAG_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT SL_LIDAR_RESP_MEASUREMENT_CHECKBIT +#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT -#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) -#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 - -typedef struct _rplidar_response_sample_rate_t { - _u16 std_sample_duration_us; - _u16 express_sample_duration_us; -} __attribute__((packed)) rplidar_response_sample_rate_t; - -typedef struct _rplidar_response_measurement_node_t { - _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; - _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; - _u16 distance_q2; -} __attribute__((packed)) rplidar_response_measurement_node_t; +typedef sl_lidar_response_sample_rate_t rplidar_response_sample_rate_t; +typedef sl_lidar_response_measurement_node_t rplidar_response_measurement_node_t; //[distance_sync flags] -#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) -#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) +#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK +#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK + +typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t; -typedef struct _rplidar_response_cabin_nodes_t { - _u16 distance_angle_1; // see [distance_sync flags] - _u16 distance_angle_2; // see [distance_sync flags] - _u8 offset_angles_q3; -} __attribute__((packed)) rplidar_response_cabin_nodes_t; +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 +#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 -#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 +typedef sl_lidar_response_capsule_measurement_nodes_t rplidar_response_capsule_measurement_nodes_t; +typedef sl_lidar_response_dense_cabin_nodes_t rplidar_response_dense_cabin_nodes_t; +typedef sl_lidar_response_dense_capsule_measurement_nodes_t rplidar_response_dense_capsule_measurement_nodes_t; -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) +// ext1 : x2 boost mode -typedef struct _rplidar_response_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_cabin_nodes_t cabins[16]; -} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t; +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS -typedef struct _rplidar_response_dense_cabin_nodes_t { - _u16 distance; -} __attribute__((packed)) rplidar_response_dense_cabin_nodes_t; +typedef sl_lidar_response_ultra_cabin_nodes_t rplidar_response_ultra_cabin_nodes_t; +typedef sl_lidar_response_ultra_capsule_measurement_nodes_t rplidar_response_ultra_capsule_measurement_nodes_t; +typedef sl_lidar_response_measurement_node_hq_t rplidar_response_measurement_node_hq_t; +typedef sl_lidar_response_hq_capsule_measurement_nodes_t rplidar_response_hq_capsule_measurement_nodes_t; -typedef struct _rplidar_response_dense_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_dense_cabin_nodes_t cabins[40]; -} __attribute__((packed)) rplidar_response_dense_capsule_measurement_nodes_t; -// ext1 : x2 boost mode +# define RPLIDAR_CONF_SCAN_COMMAND_STD SL_LIDAR_CONF_SCAN_COMMAND_STD +# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS +# define RPLIDAR_CONF_SCAN_COMMAND_HQ SL_LIDAR_CONF_SCAN_COMMAND_HQ +# define RPLIDAR_CONF_SCAN_COMMAND_BOOST SL_LIDAR_CONF_SCAN_COMMAND_BOOST +# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY SL_LIDAR_CONF_SCAN_COMMAND_STABILITY +# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY -#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 -#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 - -typedef struct _rplidar_response_ultra_cabin_nodes_t { - // 31 0 - // | predict2 10bit | predict1 10bit | major 12bit | - _u32 combined_x3; -} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t; - -typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_ultra_cabin_nodes_t ultra_cabins[32]; -} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t; - -typedef struct rplidar_response_measurement_node_hq_t { - _u16 angle_z_q14; - _u32 dist_mm_q2; - _u8 quality; - _u8 flag; -} __attribute__((packed)) rplidar_response_measurement_node_hq_t; - -typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{ - _u8 sync_byte; - _u64 time_stamp; - rplidar_response_measurement_node_hq_t node_hq[16]; - _u32 crc32; -}__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t; - - -# define RPLIDAR_CONF_SCAN_COMMAND_STD 0 -# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS 1 -# define RPLIDAR_CONF_SCAN_COMMAND_HQ 2 -# define RPLIDAR_CONF_SCAN_COMMAND_BOOST 3 -# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY 4 -# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 - -#define RPLIDAR_CONF_ANGLE_RANGE 0x00000000 -#define RPLIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 -#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 -#define RPLIDAR_CONF_MIN_ROT_FREQ 0x00000004 -#define RPLIDAR_CONF_MAX_ROT_FREQ 0x00000005 -#define RPLIDAR_CONF_MAX_DISTANCE 0x00000060 +#define RPLIDAR_CONF_ANGLE_RANGE SL_LIDAR_CONF_ANGLE_RANGE +#define RPLIDAR_CONF_DESIRED_ROT_FREQ SL_LIDAR_CONF_DESIRED_ROT_FREQ +#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP SL_LIDAR_CONF_SCAN_COMMAND_BITMAP +#define RPLIDAR_CONF_MIN_ROT_FREQ SL_LIDAR_CONF_MIN_ROT_FREQ +#define RPLIDAR_CONF_MAX_ROT_FREQ SL_LIDAR_CONF_MAX_ROT_FREQ +#define RPLIDAR_CONF_MAX_DISTANCE SL_LIDAR_CONF_MAX_DISTANCE -#define RPLIDAR_CONF_SCAN_MODE_COUNT 0x00000070 -#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 -#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 -#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 -#define RPLIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C -#define RPLIDAR_CONF_SCAN_MODE_NAME 0x0000007F -#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 -#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 - -typedef struct _rplidar_response_get_lidar_conf{ - _u32 type; - _u8 payload[0]; -}__attribute__((packed)) rplidar_response_get_lidar_conf_t; - -typedef struct _rplidar_response_set_lidar_conf{ - _u32 result; -}__attribute__((packed)) rplidar_response_set_lidar_conf_t; - - -typedef struct _rplidar_response_device_info_t { - _u8 model; - _u16 firmware_version; - _u8 hardware_version; - _u8 serialnum[16]; -} __attribute__((packed)) rplidar_response_device_info_t; - -typedef struct _rplidar_response_device_health_t { - _u8 status; - _u16 error_code; -} __attribute__((packed)) rplidar_response_device_health_t; +#define RPLIDAR_CONF_SCAN_MODE_COUNT SL_LIDAR_CONF_SCAN_MODE_COUNT +#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE +#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE +#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE +#define RPLIDAR_CONF_SCAN_MODE_TYPICAL SL_LIDAR_CONF_SCAN_MODE_TYPICAL +#define RPLIDAR_CONF_SCAN_MODE_NAME SL_LIDAR_CONF_SCAN_MODE_NAME +#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP +#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP +#define RPLIDAR_CONF_LIDAR_STATIC_IP_ADDR SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR +#define RPLIDAR_CONF_LIDAR_MAC_ADDR SL_LIDAR_CONF_LIDAR_MAC_ADDR + +#define RPLIDAR_CONF_DETECTED_SERIAL_BPS SL_LIDAR_CONF_DETECTED_SERIAL_BPS + +typedef sl_lidar_response_get_lidar_conf_t rplidar_response_get_lidar_conf_t; +typedef sl_lidar_response_set_lidar_conf_t rplidar_response_set_lidar_conf_t; +typedef sl_lidar_response_device_info_t rplidar_response_device_info_t; +typedef sl_lidar_response_device_health_t rplidar_response_device_health_t; +typedef sl_lidar_ip_conf_t rplidar_ip_conf_t; +typedef sl_lidar_response_device_macaddr_info_t rplidar_response_device_macaddr_info_t; // Definition of the variable bit scale encoding mechanism -#define RPLIDAR_VARBITSCALE_X2_SRC_BIT 9 -#define RPLIDAR_VARBITSCALE_X4_SRC_BIT 11 -#define RPLIDAR_VARBITSCALE_X8_SRC_BIT 12 -#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14 - -#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512 -#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280 -#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792 -#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328 - -#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ - ( (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ - ((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ - ((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ - ((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ - RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1) +#define RPLIDAR_VARBITSCALE_X2_SRC_BIT SL_LIDAR_VARBITSCALE_X2_SRC_BIT +#define RPLIDAR_VARBITSCALE_X4_SRC_BIT SL_LIDAR_VARBITSCALE_X4_SRC_BIT +#define RPLIDAR_VARBITSCALE_X8_SRC_BIT SL_LIDAR_VARBITSCALE_X8_SRC_BIT +#define RPLIDAR_VARBITSCALE_X16_SRC_BIT SL_LIDAR_VARBITSCALE_X16_SRC_BIT + +#define RPLIDAR_VARBITSCALE_X2_DEST_VAL SL_LIDAR_VARBITSCALE_X2_DEST_VAL +#define RPLIDAR_VARBITSCALE_X4_DEST_VAL SL_LIDAR_VARBITSCALE_X4_DEST_VAL +#define RPLIDAR_VARBITSCALE_X8_DEST_VAL SL_LIDAR_VARBITSCALE_X8_DEST_VAL +#define RPLIDAR_VARBITSCALE_X16_DEST_VAL SL_LIDAR_VARBITSCALE_X16_DEST_VAL +#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) #if defined(_WIN32) #pragma pack() diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index 9e263617..26f121a9 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -33,53 +33,22 @@ */ #pragma once - +#include "sl_lidar_driver.h" #ifndef __cplusplus #error "The RPlidar SDK requires a C++ compiler to be built" #endif -#ifndef DEPRECATED - #ifdef __GNUC__ - #define DEPRECATED(func) func __attribute__ ((deprecated)) - #elif defined(_MSC_VER) - #define DEPRECATED(func) __declspec(deprecated) func - #else - #pragma message("WARNING: You need to implement DEPRECATED for this compiler") - #define DEPRECATED(func) func - #endif -#endif namespace rp { namespace standalone{ namespace rplidar { - -struct RplidarScanMode { - _u16 id; - float us_per_sample; // microseconds per sample - float max_distance; // max distance - _u8 ans_type; // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT* - char scan_mode[64]; // name of scan mode, max 63 characters -}; - -enum { - DRIVER_TYPE_SERIALPORT = 0x0, - DRIVER_TYPE_TCP = 0x1, -}; - -class ChannelDevice -{ -public: - virtual bool bind(const char*, uint32_t ) = 0; - virtual bool open() {return true;} - virtual void close() = 0; - virtual void flush() {return;} - virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; - virtual int senddata(const _u8 * data, size_t size) = 0; - virtual int recvdata(unsigned char * data, size_t size) = 0; - virtual void setDTR() {return;} - virtual void clearDTR() {return;} - virtual void ReleaseRxTx() {return;} -}; - + using namespace sl; + typedef LidarScanMode RplidarScanMode; + +//enum { +// DRIVER_TYPE_SERIALPORT = 0x0, +// DRIVER_TYPE_TCP = 0x1, +// DRIVER_TYPE_UDP = 0x2, +//}; class RPlidarDriver { public: enum { @@ -99,13 +68,15 @@ class RPlidarDriver { /// This interface should be invoked first before any other operations /// /// \param drivertype the connection type used by the driver. - static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT); + static RPlidarDriver * CreateDriver(_u32 drivertype = CHANNEL_TYPE_SERIALPORT); + + + RPlidarDriver(sl_u32 channelType); /// Dispose the RPLIDAR Driver Instance specified by the drv parameter /// Applications should invoke this interface when the driver instance is no longer used in order to free memory static void DisposeDriver(RPlidarDriver * drv); - /// Open the specified serial port and connect to a target RPLIDAR device /// /// \param port_path the device path of the serial port @@ -117,28 +88,29 @@ class RPlidarDriver { /// /// \param flag other flags /// Reserved for future use, always set to Zero - virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0; - - + u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0); + /// Disconnect with the RPLIDAR and close the serial port - virtual void disconnect() = 0; + void disconnect(); /// Returns TRUE when the connection has been established - virtual bool isConnected() = 0; + bool isConnected(); /// Ask the RPLIDAR core system to reset it self /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. /// /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result reset(_u32 timeout = DEFAULT_TIMEOUT); - virtual u_result clearNetSerialRxCache() = 0; + u_result clearNetSerialRxCache() { + return RESULT_OK; + } // FW1.24 /// Get all scan modes that supported by lidar - virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; - + u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); + /// Get typical scan mode of lidar - virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); /// Start scan /// @@ -146,7 +118,7 @@ class RPlidarDriver { /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) /// \param options Scan options (please use 0) /// \param outUsedScanMode The scan mode selected by lidar - virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0; + u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); /// Start scan in specific mode /// @@ -154,7 +126,7 @@ class RPlidarDriver { /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) /// \param options Scan options (please use 0) /// \param outUsedScanMode The scan mode selected by lidar - virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); /// Retrieve the health status of the RPLIDAR /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. @@ -162,111 +134,47 @@ class RPlidarDriver { /// \param health The health status info returned from the RPLIDAR /// /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. /// /// \param info The device information returned from the RPLIDAR /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); - /// Get the sample duration information of the RPLIDAR. - /// DEPRECATED, please use RplidarScanMode::us_per_sample - /// - /// \param rateInfo The sample duration information returned from the RPLIDAR - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0; - - /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 and A3 only. + /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only. /// /// \param pwm The motor pwm value would like to set - virtual u_result setMotorPWM(_u16 pwm) = 0; - - /// Set the RPLIDAR's motor rpm, currently valid for tof lidar only. - /// - /// \param rpm The motor rpm value would like to set - virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result setMotorPWM(_u16 pwm); /// Start RPLIDAR's motor when using accessory board - virtual u_result startMotor() = 0; + u_result startMotor(); /// Stop RPLIDAR's motor when using accessory board - virtual u_result stopMotor() = 0; + u_result stopMotor(); /// Check whether the device support motor control. /// Note: this API will disable grab. /// /// \param support Return the result. /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); - /// Check if the device is Tof lidar. - /// Note: this API is effective if and only if getDeviceInfo has been called. - /// - /// \param support Return the result. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT) = 0; + ///Set LPX series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + u_result setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); - /// Calculate RPLIDAR's current scanning frequency from the given scan data - /// DEPRECATED, please use getFrequency(RplidarScanMode, size_t) - /// - /// Please refer to the application note doc for details - /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data - /// - /// \param inExpressMode Indicate whether the RPLIDAR is in express mode - /// \param count The number of sample nodes inside the given buffer - /// \param frequency The scanning frequency (in HZ) calcuated by the interface. - /// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode. - DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0; - - /// Calculate RPLIDAR's current scanning frequency from the given scan data - /// Please refer to the application note doc for details - /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data - /// - /// \param scanMode Lidar's current scan mode - /// \param count The number of sample nodes inside the given buffer - virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0; - - /// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) - /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously. - /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread. - /// - /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Check whether the device support express mode. - /// DEPRECATED, please use getAllSupportedScanModes - /// - /// \param support Return the result. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT)) = 0; + ///Get LPX series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT); /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated /// /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0; - - - /// Wait and grab a complete 0-360 degree scan data previously received. - /// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq - /// The grabbed scan data returned by this interface always has the following charactistics: - /// - /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 - /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan - /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. - /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// - /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. - /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. - /// - /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. - DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT)) = 0; + u_result stop(_u32 timeout = DEFAULT_TIMEOUT); /// Wait and grab a complete 0-360 degree scan data previously received. /// The grabbed scan data returned by this interface always has the following charactistics: @@ -285,7 +193,7 @@ class RPlidarDriver { /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. /// /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. - virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; + u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); /// Ascending the scan data according to the angle value in the scan. /// @@ -294,16 +202,7 @@ class RPlidarDriver { /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). /// Once the interface returns, this parameter will store the actual received data count. /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. - DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0; - - /// Ascending the scan data according to the angle value in the scan. - /// - /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. - virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0; + u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); /// Return received scan points even if it's not complete scan /// @@ -312,26 +211,27 @@ class RPlidarDriver { /// \param count Once the interface returns, this parameter will store the actual received data count. /// /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. - DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0; + u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); - /// Return received scan points even if it's not complete scan. + /// Return received scan points even if it's not complete scan /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data. This buffer must be initialized by - /// the caller. + /// \param nodebuffer Buffer provided by the caller application to store the scan data /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. + /// \param count Once the interface returns, this parameter will store the actual received data count. /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. - /// The interface will return RESULT_REMAINING_DATA to indicate that the given buffer is full, but that there remains data to be read. - virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0; + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + - virtual ~RPlidarDriver() {} + virtual ~RPlidarDriver(); protected: - RPlidarDriver(){} + RPlidarDriver(); -public: - ChannelDevice* _chanDev; +private: + sl_u32 _channelType; + IChannel* _channel; + ILidarDriver* _lidarDrv; + }; diff --git a/sdk/include/rplidar_protocol.h b/sdk/include/rplidar_protocol.h index 2f8231b7..5b17bbb8 100644 --- a/sdk/include/rplidar_protocol.h +++ b/sdk/include/rplidar_protocol.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,39 +33,27 @@ */ #pragma once - +#include "sl_lidar_protocol.h" // RP-Lidar Input Packets -#define RPLIDAR_CMD_SYNC_BYTE 0xA5 -#define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80 +#define RPLIDAR_CMD_SYNC_BYTE SL_LIDAR_CMD_SYNC_BYTE +#define RPLIDAR_CMDFLAG_HAS_PAYLOAD SL_LIDAR_CMDFLAG_HAS_PAYLOAD -#define RPLIDAR_ANS_SYNC_BYTE1 0xA5 -#define RPLIDAR_ANS_SYNC_BYTE2 0x5A +#define RPLIDAR_ANS_SYNC_BYTE1 SL_LIDAR_ANS_SYNC_BYTE1 +#define RPLIDAR_ANS_SYNC_BYTE2 SL_LIDAR_ANS_SYNC_BYTE2 -#define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 +#define RPLIDAR_ANS_PKTFLAG_LOOP SL_LIDAR_ANS_PKTFLAG_LOOP -#define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF -#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) +#define RPLIDAR_ANS_HEADER_SIZE_MASK SL_LIDAR_ANS_HEADER_SIZE_MASK +#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT #if defined(_WIN32) #pragma pack(1) #endif -typedef struct _rplidar_cmd_packet_t { - _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE - _u8 cmd_flag; - _u8 size; - _u8 data[0]; -} __attribute__((packed)) rplidar_cmd_packet_t; - - -typedef struct _rplidar_ans_header_t { - _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 - _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 - _u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; - _u8 type; -} __attribute__((packed)) rplidar_ans_header_t; +typedef sl_lidar_cmd_packet_t rplidar_cmd_packet_t; +typedef sl_lidar_ans_header_t rplidar_ans_header_t; #if defined(_WIN32) #pragma pack() diff --git a/sdk/include/sl_crc.h b/sdk/include/sl_crc.h new file mode 100644 index 00000000..184a8df9 --- /dev/null +++ b/sdk/include/sl_crc.h @@ -0,0 +1,43 @@ +/* +* Slamtec LIDAR SDK +* +* sl_crc.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_cmd.h" + +namespace sl {namespace crc32 { + sl_u32 bitrev(sl_u32 input, sl_u16 bw);//reflect + void init(sl_u32 poly); // table init + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len); + sl_result getResult(sl_u8 *ptr, sl_u32 len); +}} diff --git a/sdk/include/sl_lidar.h b/sdk/include/sl_lidar.h new file mode 100644 index 00000000..cc672371 --- /dev/null +++ b/sdk/include/sl_lidar.h @@ -0,0 +1,42 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_driver.h" + +#define SL_LIDAR_SDK_VERSION_MAJOR 2 +#define SL_LIDAR_SDK_VERSION_MINOR 0 +#define SL_LIDAR_SDK_VERSION_PATCH 0 +#define SL_LIDAR_SDK_VERSION_SEQ ((SL_LIDAR_SDK_VERSION_MAJOR << 16) | (SL_LIDAR_SDK_VERSION_MINOR << 8) | SL_LIDAR_SDK_VERSION_PATCH) +#define SL_LIDAR_SDK_VERSION (#SL_LIDAR_SDK_VERSION_MAJOR "." #SL_LIDAR_SDK_VERSION_MINOR "." #SL_LIDAR_SDK_VERSION_PATCH) diff --git a/sdk/include/sl_lidar_cmd.h b/sdk/include/sl_lidar_cmd.h new file mode 100644 index 00000000..ac9ea143 --- /dev/null +++ b/sdk/include/sl_lidar_cmd.h @@ -0,0 +1,354 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_cmd.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_protocol.h" + + // Commands + //----------------------------------------- + + +#define SL_LIDAR_AUTOBAUD_MAGICBYTE 0x41 + + // Commands without payload and response +#define SL_LIDAR_CMD_STOP 0x25 +#define SL_LIDAR_CMD_SCAN 0x20 +#define SL_LIDAR_CMD_FORCE_SCAN 0x21 +#define SL_LIDAR_CMD_RESET 0x40 + +// Commands with payload but no response +#define SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM 0x90 //added in fw 1.30 + +// Commands without payload but have response +#define SL_LIDAR_CMD_GET_DEVICE_INFO 0x50 +#define SL_LIDAR_CMD_GET_DEVICE_HEALTH 0x52 + +#define SL_LIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 + +#define SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 + + +// Commands with payload and have response +#define SL_LIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 +#define SL_LIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 +#define SL_LIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 +#define SL_LIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define SL_LIDAR_CMD_SET_MOTOR_PWM 0xF0 +#define SL_LIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Payloads +// ------------------------------------------ +#define SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL 0 +#define SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail +//for express working flag(extending express scan protocol) +#define SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 +#define SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 + +//for ultra express working flag +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 + +typedef struct _sl_lidar_payload_express_scan_t +{ + sl_u8 working_mode; + sl_u16 working_flags; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_express_scan_t; + +typedef struct _sl_lidar_payload_hq_scan_t +{ + sl_u8 flag; + sl_u8 reserved[32]; +} __attribute__((packed)) sl_lidar_payload_hq_scan_t; + +typedef struct _sl_lidar_payload_get_scan_conf_t +{ + sl_u32 type; + sl_u8 reserved[32]; +} __attribute__((packed)) sl_lidar_payload_get_scan_conf_t; + +typedef struct _sl_payload_set_scan_conf_t { + sl_u32 type; +} __attribute__((packed)) sl_lidar_payload_set_scan_conf_t; + + +#define DEFAULT_MOTOR_SPEED (0xFFFFu) + +typedef struct _sl_lidar_payload_motor_pwm_t +{ + sl_u16 pwm_value; +} __attribute__((packed)) sl_lidar_payload_motor_pwm_t; + +typedef struct _sl_lidar_payload_acc_board_flag_t +{ + sl_u32 reserved; +} __attribute__((packed)) sl_lidar_payload_acc_board_flag_t; + +typedef struct _sl_lidar_payload_hq_spd_ctrl_t { + sl_u16 rpm; +} __attribute__((packed))sl_lidar_payload_hq_spd_ctrl_t; + + +typedef struct _sl_lidar_payload_new_bps_confirmation_t { + sl_u16 flag; // reserved, must be 0x5F5F + sl_u32 required_bps; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_new_bps_confirmation_t; + +// Response +// ------------------------------------------ +#define SL_LIDAR_ANS_TYPE_DEVINFO 0x4 +#define SL_LIDAR_ANS_TYPE_DEVHEALTH 0x6 + +#define SL_LIDAR_ANS_TYPE_MEASUREMENT 0x81 +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 + + +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE 0x15 +//added in FW ver 1.23alpha +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 +//added in FW ver 1.24 +#define SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 +#define SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 +#define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF + +#define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) +typedef struct _sl_lidar_response_acc_board_flag_t +{ + sl_u32 support_flag; +} __attribute__((packed)) sl_lidar_response_acc_board_flag_t; + + +#define SL_LIDAR_STATUS_OK 0x0 +#define SL_LIDAR_STATUS_WARNING 0x1 +#define SL_LIDAR_STATUS_ERROR 0x2 + +#define SL_LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 + +#define SL_LIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) + +#define SL_LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +typedef struct _sl_lidar_response_sample_rate_t +{ + sl_u16 std_sample_duration_us; + sl_u16 express_sample_duration_us; +} __attribute__((packed)) sl_lidar_response_sample_rate_t; + +typedef struct _sl_lidar_response_measurement_node_t +{ + sl_u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; + sl_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; + sl_u16 distance_q2; +} __attribute__((packed)) sl_lidar_response_measurement_node_t; + +//[distance_sync flags] +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) +#define SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) + +typedef struct _sl_lidar_response_cabin_nodes_t +{ + sl_u16 distance_angle_1; // see [distance_sync flags] + sl_u16 distance_angle_2; // see [distance_sync flags] + sl_u8 offset_angles_q3; +} __attribute__((packed)) sl_lidar_response_cabin_nodes_t; + + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 + +#define SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) + +typedef struct _sl_lidar_response_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_cabin_nodes_t cabins[16]; +} __attribute__((packed)) sl_lidar_response_capsule_measurement_nodes_t; + +typedef struct _sl_lidar_response_dense_cabin_nodes_t +{ + sl_u16 distance; +} __attribute__((packed)) sl_lidar_response_dense_cabin_nodes_t; + +typedef struct _sl_lidar_response_dense_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_dense_cabin_nodes_t cabins[40]; +} __attribute__((packed)) sl_lidar_response_dense_capsule_measurement_nodes_t; + +// ext1 : x2 boost mode + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 + +typedef struct _sl_lidar_response_ultra_cabin_nodes_t +{ + // 31 0 + // | predict2 10bit | predict1 10bit | major 12bit | + sl_u32 combined_x3; +} __attribute__((packed)) sl_lidar_response_ultra_cabin_nodes_t; + +typedef struct _sl_lidar_response_ultra_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_ultra_cabin_nodes_t ultra_cabins[32]; +} __attribute__((packed)) sl_lidar_response_ultra_capsule_measurement_nodes_t; + +typedef struct sl_lidar_response_measurement_node_hq_t +{ + sl_u16 angle_z_q14; + sl_u32 dist_mm_q2; + sl_u8 quality; + sl_u8 flag; +} __attribute__((packed)) sl_lidar_response_measurement_node_hq_t; + +typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t +{ + sl_u8 sync_byte; + sl_u64 time_stamp; + sl_lidar_response_measurement_node_hq_t node_hq[96]; + sl_u32 crc32; +}__attribute__((packed)) sl_lidar_response_hq_capsule_measurement_nodes_t; + + +# define SL_LIDAR_CONF_SCAN_COMMAND_STD 0 +# define SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS 1 +# define SL_LIDAR_CONF_SCAN_COMMAND_HQ 2 +# define SL_LIDAR_CONF_SCAN_COMMAND_BOOST 3 +# define SL_LIDAR_CONF_SCAN_COMMAND_STABILITY 4 +# define SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 + +#define SL_LIDAR_CONF_ANGLE_RANGE 0x00000000 +#define SL_LIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 +#define SL_LIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 +#define SL_LIDAR_CONF_MIN_ROT_FREQ 0x00000004 +#define SL_LIDAR_CONF_MAX_ROT_FREQ 0x00000005 +#define SL_LIDAR_CONF_MAX_DISTANCE 0x00000060 + +#define SL_LIDAR_CONF_SCAN_MODE_COUNT 0x00000070 +#define SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 +#define SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 +#define SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 +#define SL_LIDAR_CONF_LIDAR_MAC_ADDR 0x00000079 +#define SL_LIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C +#define SL_LIDAR_CONF_SCAN_MODE_NAME 0x0000007F +#define SL_LIDAR_CONF_DETECTED_SERIAL_BPS 0x000000A1 + +#define SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR 0x0001CCC0 +#define SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 +#define SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 + +typedef struct _sl_lidar_response_get_lidar_conf +{ + sl_u32 type; + sl_u8 payload[0]; +}__attribute__((packed)) sl_lidar_response_get_lidar_conf_t; + +typedef struct _sl_lidar_response_set_lidar_conf +{ + sl_u32 result; +}__attribute__((packed)) sl_lidar_response_set_lidar_conf_t; + + +typedef struct _sl_lidar_response_device_info_t +{ + sl_u8 model; + sl_u16 firmware_version; + sl_u8 hardware_version; + sl_u8 serialnum[16]; +} __attribute__((packed)) sl_lidar_response_device_info_t; + +typedef struct _sl_lidar_response_device_health_t +{ + sl_u8 status; + sl_u16 error_code; +} __attribute__((packed)) sl_lidar_response_device_health_t; + +typedef struct _sl_lidar_ip_conf_t { + sl_u8 ip_addr[4]; + sl_u8 net_mask[4]; + sl_u8 gw[4]; +}__attribute__((packed)) sl_lidar_ip_conf_t; + +typedef struct _sl_lidar_response_device_macaddr_info_t { + sl_u8 macaddr[6]; +} __attribute__((packed)) sl_lidar_response_device_macaddr_info_t; + +typedef struct _sl_lidar_response_desired_rot_speed_t{ + sl_u16 rpm; + sl_u16 pwm_ref; +}__attribute__((packed)) sl_lidar_response_desired_rot_speed_t; + +// Definition of the variable bit scale encoding mechanism +#define SL_LIDAR_VARBITSCALE_X2_SRC_BIT 9 +#define SL_LIDAR_VARBITSCALE_X4_SRC_BIT 11 +#define SL_LIDAR_VARBITSCALE_X8_SRC_BIT 12 +#define SL_LIDAR_VARBITSCALE_X16_SRC_BIT 14 + +#define SL_LIDAR_VARBITSCALE_X2_DEST_VAL 512 +#define SL_LIDAR_VARBITSCALE_X4_DEST_VAL 1280 +#define SL_LIDAR_VARBITSCALE_X8_DEST_VAL 1792 +#define SL_LIDAR_VARBITSCALE_X16_DEST_VAL 3328 + +#define SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ + ( (((0x1<<(_BITS_)) - SL_LIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ + ((SL_LIDAR_VARBITSCALE_X16_DEST_VAL - SL_LIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ + ((SL_LIDAR_VARBITSCALE_X8_DEST_VAL - SL_LIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ + ((SL_LIDAR_VARBITSCALE_X4_DEST_VAL - SL_LIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ + SL_LIDAR_VARBITSCALE_X2_DEST_VAL - 1) + + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/sdk/include/sl_lidar_driver.h b/sdk/include/sl_lidar_driver.h new file mode 100644 index 00000000..c1dbe241 --- /dev/null +++ b/sdk/include/sl_lidar_driver.h @@ -0,0 +1,445 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifndef __cplusplus +#error "The Slamtec LIDAR SDK requires a C++ compiler to be built" +#endif + +#include +#include +#include + +#ifndef DEPRECATED + #ifdef __GNUC__ + #define DEPRECATED(func) func __attribute__ ((deprecated)) + #elif defined(_MSC_VER) + #define DEPRECATED(func) __declspec(deprecated) func + #else + #pragma message("WARNING: You need to implement DEPRECATED for this compiler") + #define DEPRECATED(func) func + #endif +#endif + + +#include "sl_lidar_cmd.h" + +#include + +namespace sl { + +#ifdef DEPRECATED +#define DEPRECATED_WARN(fn, replacement) do { \ + static bool __shown__ = false; \ + if (!__shown__) { \ + printDeprecationWarn(fn, replacement); \ + __shown__ = true; \ + } \ + } while (0) +#endif + + /** + * Lidar scan mode + */ + struct LidarScanMode + { + // Mode id + sl_u16 id; + + // Time cost for one measurement (in microseconds) + float us_per_sample; + + // Max distance in this scan mode (in meters) + float max_distance; + + // The answer command code for this scan mode + sl_u8 ans_type; + + // The name of scan mode (padding with 0 if less than 64 characters) + char scan_mode[64]; + }; + + template + struct Result + { + sl_result err; + T value; + Result(const T& value) + : err(SL_RESULT_OK) + , value(value) + { + } + + Result(sl_result err) + : err(err) + , value() + { + } + + operator sl_result() const + { + return err; + } + + operator bool() const + { + return SL_IS_OK(err); + } + + T& operator* () + { + return value; + } + + T* operator-> () + { + return &value; + } + }; + + /** + * Abstract interface of communication channel + */ + class IChannel + { + public: + virtual ~IChannel() {} + + public: + /** + * Open communication channel (return true if succeed) + */ + virtual bool open() = 0; + + /** + * Close communication channel + */ + virtual void close() = 0; + + /** + * Flush all written data to remote endpoint + */ + virtual void flush() = 0; + + /** + * Wait for some data + * \param size Bytes to wait + * \param timeoutInMs Wait timeout (in microseconds, -1 for forever) + * \param actualReady [out] actual ready bytes + * \return true for data ready + */ + virtual bool waitForData(size_t size, sl_u32 timeoutInMs = -1, size_t* actualReady = nullptr) = 0; + + /** + * Send data to remote endpoint + * \param data The data buffer + * \param size The size of data buffer (in bytes) + * \return Bytes written (negative for write failure) + */ + virtual int write(const void* data, size_t size) = 0; + + /** + * Read data from the chanel + * \param buffer The buffer to receive data + * \param size The size of the read buffer + * \return Bytes read (negative for read failure) + */ + virtual int read(void* buffer, size_t size) = 0; + + /** + * Clear read cache + */ + virtual void clearReadCache() = 0; + + private: + + }; + + /** + * Abstract interface of serial port channel + */ + class ISerialPortChannel : public IChannel + { + public: + virtual ~ISerialPortChannel() {} + + public: + virtual void setDTR(bool dtr) = 0; + }; + + /** + * Create a serial channel + * \param device Serial port device + * e.g. on Windows, it may be com3 or \\.\com10 + * on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + * \param baudrate Baudrate + * Please refer to the datasheet for the baudrate (maybe 115200 or 256000) + */ + Result createSerialPortChannel(const std::string& device, int baudrate); + + /** + * Create a TCP channel + * \param ip IP address of the device + * \param port TCP port + */ + Result createTcpChannel(const std::string& ip, int port); + + /** + * Create a UDP channel + * \param ip IP address of the device + * \param port UDP port + */ + Result createUdpChannel(const std::string& ip, int port); + + enum MotorCtrlSupport + { + MotorCtrlSupportNone = 0, + MotorCtrlSupportPwm = 1, + MotorCtrlSupportRpm = 2, + }; + + enum ChannelType{ + CHANNEL_TYPE_SERIALPORT = 0x0, + CHANNEL_TYPE_TCP = 0x1, + CHANNEL_TYPE_UDP = 0x2, + }; + + /** + * Lidar motor info + */ + struct LidarMotorInfo + { + MotorCtrlSupport motorCtrlSupport; + + // Desire speed + sl_u16 desired_speed; + + // Max speed + sl_u16 max_speed; + + // Min speed + sl_u16 min_speed; + }; + + class ILidarDriver + { + public: + virtual ~ILidarDriver() {} + + public: + /** + * Connect to LIDAR via channel + * \param channel The communication channel + * Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle + */ + virtual sl_result connect(IChannel* channel) = 0; + + /** + * Disconnect from the LIDAR + */ + virtual void disconnect() = 0; + + /** + * Check if the connection is established + */ + virtual bool isConnected() = 0; + + public: + enum + { + DEFAULT_TIMEOUT = 2000 + }; + + public: + /// Ask the LIDAR core system to reset it self + /// The host system can use the Reset operation to help LIDAR escape the self-protection mode. + /// + /// \param timeout The operation timeout value (in millisecond) + virtual sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get all scan modes that supported by lidar + virtual sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get typical scan mode of lidar + virtual sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Start scan + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) = 0; + + /// Start scan in specific mode + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Retrieve the health status of the RPLIDAR + /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. + /// + /// \param info The device information returned from the RPLIDAR + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Check whether the device support motor control + /// Note: this API will disable grab. + /// + /// \param motorCtrlSupport Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual sl_result checkMotorCtrlSupport(MotorCtrlSupport& motorCtrlSupport, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Calculate LIDAR's current scanning frequency from the given scan data + /// Please refer to the application note doc for details + /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data + /// + /// \param scanMode Lidar's current scan mode + /// \param nodes Current scan's measurements + /// \param count The number of sample nodes inside the given buffer + virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0; + + ///Set LPX series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + ///Get LPX series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Wait and grab a complete 0-360 degree scan data previously received. + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + virtual sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Ascending the scan data according to the angle value in the scan. + /// + /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. + virtual sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t count) = 0; + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + virtual sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count) = 0; + /// Set lidar motor speed + /// The host system can use this operation to set lidar motor speed. + /// + /// \param speed The speed value set to lidar + /// + ///Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED. + virtual sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) = 0; + + /// Get the motor information of the RPLIDAR include the max speed, min speed, desired speed. + /// + /// \param motorInfo The motor information returned from the RPLIDAR + virtual sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + + /// Ask the LIDAR to use a new baudrate for serial communication + /// The target LIDAR system must support such feature to work. + /// This function does NOT check whether the target LIDAR works with the requiredBaudRate or not. + /// In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead. + /// + /// \param requiredBaudRate The new baudrate required to be used. It MUST matches with the baudrate of the binded channel. + /// \param baudRateDetected The actual baudrate detected by the LIDAR system + virtual sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL) = 0; +}; + + /** + * Create a LIDAR driver instance + * + * Example + * Result channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + * assert((bool)channel); + * assert(*channel); + * + * auto lidar = createLidarDriver(); + * assert((bool)lidar); + * assert(*lidar); + * + * auto res = (*lidar)->connect(*channel); + * assert(SL_IS_OK(res)); + * + * sl_lidar_response_device_info_t deviceInfo; + * res = (*lidar)->getDeviceInfo(deviceInfo); + * assert(SL_IS_OK(res)); + * + * printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n", + * deviceInfo.model, + * deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu, + * deviceInfo.hardware_version); + * + * delete *lidar; + * delete *channel; + */ + Result createLidarDriver(); +} diff --git a/sdk/include/sl_lidar_driver_impl.h b/sdk/include/sl_lidar_driver_impl.h new file mode 100644 index 00000000..77cc85d1 --- /dev/null +++ b/sdk/include/sl_lidar_driver_impl.h @@ -0,0 +1,150 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +namespace sl { + class SL_LidarDriver :public ILidarDriver + { + public: + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + + enum + { + NORMAL_CAPSULE = 0, + DENSE_CAPSULE = 1, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + TOF_LIDAR_MINUM_MAJOR_ID = 6, + }; + public: + SL_LidarDriver() + :_channel(NULL) + , _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _cached_sampleduration_std(LEGACY_SAMPLE_DURATION) + ,_cached_sampleduration_express(LEGACY_SAMPLE_DURATION) + , _cached_scan_node_hq_count(0) + , _cached_scan_node_hq_count_for_interval_retrieve(0) + {} + + sl_result connect(IChannel* channel); + void disconnect(); + bool isConnected(); + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr); + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT); + DEPRECATED(sl_result grabScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT)); + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency); + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout); + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs); + sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count); + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count); + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_PWM);// + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL); + + protected: + sl_result startMotor(); + sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout); + sl_result getLidarConf(sl_u32 type, std::vector &outputBuf, const std::vector &reserve = std::vector(), sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + //DEPRECATED(sl_result getSampleDuration_uS(sl_lidar_response_sample_rate_t & rateInfo, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED (sl_result checkExpressScanSupported(bool & support, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED(sl_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)); + private: + sl_result _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 ); + sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT); + template + sl_result _waitResponse(T &payload ,sl_u8 ansType, _u32 timeout = DEFAULT_TIMEOUT); + void _disableDataGrabbing(); + sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheScanData(); + void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheCapsuledScanData(); + sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheHqScanData(); + sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheUltraCapsuledScanData(); + sl_result _clearRxDataCache(); + + private: + IChannel *_channel; + bool _isConnected; + bool _isScanning; + MotorCtrlSupport _isSupportingMotorCtrl; + + rp::hal::Locker _lock; + rp::hal::Event _dataEvt; + rp::hal::Thread _cachethread; + sl_u16 _cached_sampleduration_std; + sl_u16 _cached_sampleduration_express; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; + size_t _cached_scan_node_hq_count; + sl_u8 _cached_capsule_flag; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_hq_count_for_interval_retrieve; + + sl_lidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; + bool _is_previous_capsuledataRdy; + bool _is_previous_HqdataRdy; + }; + +} diff --git a/sdk/include/sl_lidar_protocol.h b/sdk/include/sl_lidar_protocol.h new file mode 100644 index 00000000..5337ba9a --- /dev/null +++ b/sdk/include/sl_lidar_protocol.h @@ -0,0 +1,72 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_protocol.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_types.h" + +#define SL_LIDAR_CMD_SYNC_BYTE 0xA5 +#define SL_LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 + +#define SL_LIDAR_ANS_SYNC_BYTE1 0xA5 +#define SL_LIDAR_ANS_SYNC_BYTE2 0x5A + +#define SL_LIDAR_ANS_PKTFLAG_LOOP 0x1 + +#define SL_LIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF +#define SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) + +#if defined(_WIN32) +#pragma pack(1) +#endif + +typedef struct sl_lidar_cmd_packet_t +{ + sl_u8 syncByte; //must be SL_LIDAR_CMD_SYNC_BYTE + sl_u8 cmd_flag; + sl_u8 size; + sl_u8 data[0]; +} __attribute__((packed)) sl_lidar_cmd_packet_t; + + +typedef struct sl_lidar_ans_header_t +{ + sl_u8 syncByte1; // must be SL_LIDAR_ANS_SYNC_BYTE1 + sl_u8 syncByte2; // must be SL_LIDAR_ANS_SYNC_BYTE2 + sl_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; + sl_u8 type; +} __attribute__((packed)) sl_lidar_ans_header_t; + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/sdk/include/sl_types.h b/sdk/include/sl_types.h new file mode 100644 index 00000000..96a37556 --- /dev/null +++ b/sdk/include/sl_types.h @@ -0,0 +1,83 @@ +/* +* Slamtec LIDAR SDK +* +* sl_types.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __cplusplus +#include + +#define SL_DEFINE_TYPE(IntType, NewType) typedef std::IntType NewType +#else +#include + +#define SL_DEFINE_TYPE(IntType, NewType) typedef IntType NewType +#endif + +#define SL_DEFINE_INT_TYPE(Bits) \ + SL_DEFINE_TYPE(int ## Bits ## _t, sl_s ## Bits); \ + SL_DEFINE_TYPE(uint ## Bits ## _t, sl_u ## Bits); \ + +SL_DEFINE_INT_TYPE(8) +SL_DEFINE_INT_TYPE(16) +SL_DEFINE_INT_TYPE(32) +SL_DEFINE_INT_TYPE(64) + +#if !defined(__GNUC__) && !defined(__attribute__) +# define __attribute__(x) +#endif + +#ifdef WIN64 +typedef sl_u64 sl_word_size_t; +#elif defined(WIN32) +typedef sl_u32 sl_word_size_t; +#elif defined(__GNUC__) +typedef unsigned long sl_word_size_t; +#elif defined(__ICCARM__) +typedef sl_u32 sl_word_size_t; +#endif + +typedef uint32_t sl_result; + +#define SL_RESULT_OK (sl_result)0 +#define SL_RESULT_FAIL_BIT (sl_result)0x80000000 +#define SL_RESULT_ALREADY_DONE (sl_result)0x20 +#define SL_RESULT_INVALID_DATA (sl_result)(0x8000 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_FAIL (sl_result)(0x8001 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_TIMEOUT (sl_result)(0x8002 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_STOP (sl_result)(0x8003 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_NOT_SUPPORT (sl_result)(0x8004 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_FORMAT_NOT_SUPPORT (sl_result)(0x8005 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_INSUFFICIENT_MEMORY (sl_result)(0x8006 | SL_RESULT_FAIL_BIT) + +#define SL_IS_OK(x) ( ((x) & SL_RESULT_FAIL_BIT) == 0 ) +#define SL_IS_FAIL(x) ( ((x) & SL_RESULT_FAIL_BIT) ) diff --git a/sdk/src/arch/linux/net_socket.cpp b/sdk/src/arch/linux/net_socket.cpp index 0124a13a..36b9bec8 100644 --- a/sdk/src/arch/linux/net_socket.cpp +++ b/sdk/src/arch/linux/net_socket.cpp @@ -719,7 +719,7 @@ class _single_thread DGramSocketImpl : public DGramSocket virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) { - const struct sockaddr * addr = reinterpret_cast(target.getPlatformData()); + const struct sockaddr* addr = &target ? reinterpret_cast(target.getPlatformData()) : NULL; assert(addr); size_t ans = ::sendto( _socket_fd, buffer, len, 0, addr, sizeof(sockaddr_storage)); if (ans != (size_t)-1) { @@ -743,6 +743,16 @@ class _single_thread DGramSocketImpl : public DGramSocket } + virtual u_result setPairAddress(const SocketAddress* pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast(pairAddress->getPlatformData()) : reinterpret_cast(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + + } virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) { diff --git a/sdk/src/arch/win32/arch_win32.h b/sdk/src/arch/win32/arch_win32.h index 10852f36..3ae66550 100644 --- a/sdk/src/arch/win32/arch_win32.h +++ b/sdk/src/arch/win32/arch_win32.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/arch/win32/net_serial.cpp b/sdk/src/arch/win32/net_serial.cpp index 7a35a172..c2dae7c1 100644 --- a/sdk/src/arch/win32/net_serial.cpp +++ b/sdk/src/arch/win32/net_serial.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/arch/win32/net_serial.h b/sdk/src/arch/win32/net_serial.h index a390f94c..d43137e2 100644 --- a/sdk/src/arch/win32/net_serial.h +++ b/sdk/src/arch/win32/net_serial.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/arch/win32/net_socket.cpp b/sdk/src/arch/win32/net_socket.cpp index c2968b6f..1121a6a7 100644 --- a/sdk/src/arch/win32/net_socket.cpp +++ b/sdk/src/arch/win32/net_socket.cpp @@ -767,12 +767,36 @@ class _single_thread DGramSocketImpl : public DGramSocket return RESULT_OPERATION_FAIL; } } - +/* virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) { const struct sockaddr * addr = reinterpret_cast(target.getPlatformData()); + //const struct sockaddr* addr = &target ? reinterpret_cast(target.getPlatformData()) : NULL; + //int dest_addr_size = (&target ? sizeof(sockaddr_storage) : 0); assert(addr); int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, (int)sizeof(sockaddr_storage)); + //int ans = ::sendto(_socket_fd, (const char*)buffer, (int)len, 0, addr, dest_addr_size); + if (ans != SOCKET_ERROR) { + assert(ans == (int)len); + return RESULT_OK; + } else { + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + case WSAEMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + } + + } +*/ + virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) + { + const struct sockaddr* addr = &target ? reinterpret_cast(target.getPlatformData()) : NULL; + int dest_addr_size = (&target ? sizeof(sockaddr_storage) : 0); + int ans = ::sendto(_socket_fd, (const char*)buffer, (int)len, 0, addr, dest_addr_size); if (ans != SOCKET_ERROR) { assert(ans == (int)len); return RESULT_OK; @@ -787,6 +811,17 @@ class _single_thread DGramSocketImpl : public DGramSocket } } + } + + virtual u_result setPairAddress(const SocketAddress *pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast(pairAddress->getPlatformData()) : reinterpret_cast(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + } diff --git a/sdk/src/arch/win32/timer.cpp b/sdk/src/arch/win32/timer.cpp index b95c0542..1d7c15fc 100644 --- a/sdk/src/arch/win32/timer.cpp +++ b/sdk/src/arch/win32/timer.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/arch/win32/timer.h b/sdk/src/arch/win32/timer.h index 59b3982f..c5207717 100644 --- a/sdk/src/arch/win32/timer.h +++ b/sdk/src/arch/win32/timer.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/arch/win32/winthread.hpp b/sdk/src/arch/win32/winthread.hpp index 604f1d39..ea0e05f9 100644 --- a/sdk/src/arch/win32/winthread.hpp +++ b/sdk/src/arch/win32/winthread.hpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/abs_rxtx.h b/sdk/src/hal/abs_rxtx.h index 3c0e8373..54900e31 100644 --- a/sdk/src/hal/abs_rxtx.h +++ b/sdk/src/hal/abs_rxtx.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/event.h b/sdk/src/hal/event.h index 5e05234d..de6ceaf5 100644 --- a/sdk/src/hal/event.h +++ b/sdk/src/hal/event.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/locker.h b/sdk/src/hal/locker.h index d7a13fbf..dd33a84d 100644 --- a/sdk/src/hal/locker.h +++ b/sdk/src/hal/locker.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/socket.h b/sdk/src/hal/socket.h index 55f94b59..d1ea4582 100644 --- a/sdk/src/hal/socket.h +++ b/sdk/src/hal/socket.h @@ -135,7 +135,7 @@ class _single_thread DGramSocket: public SocketBase static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); - + virtual u_result setPairAddress(const SocketAddress* pairAddress) = 0; virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) = 0; diff --git a/sdk/src/hal/thread.cpp b/sdk/src/hal/thread.cpp index 16ccb905..bc68dd5f 100644 --- a/sdk/src/hal/thread.cpp +++ b/sdk/src/hal/thread.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/thread.h b/sdk/src/hal/thread.h index 94074b8c..9f1286ef 100644 --- a/sdk/src/hal/thread.h +++ b/sdk/src/hal/thread.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/hal/types.h b/sdk/src/hal/types.h index f603e678..c9bb9021 100644 --- a/sdk/src/hal/types.h +++ b/sdk/src/hal/types.h @@ -88,7 +88,6 @@ typedef uint32_t u_result; #define RESULT_OK 0 #define RESULT_FAIL_BIT 0x80000000 #define RESULT_ALREADY_DONE 0x20 -#define RESULT_REMAINING_DATA 0x21 #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) diff --git a/sdk/src/hal/util.h b/sdk/src/hal/util.h index 2882b1d6..205a858d 100644 --- a/sdk/src/hal/util.h +++ b/sdk/src/hal/util.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index 65d2c22a..29b35ed7 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -1,2304 +1,194 @@ /* - * RPLIDAR SDK + * Slamtec LIDAR SDK * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include "sdkcommon.h" - -#include "hal/abs_rxtx.h" -#include "hal/thread.h" -#include "hal/types.h" -#include "hal/assert.h" -#include "hal/locker.h" -#include "hal/socket.h" -#include "hal/event.h" -#include "rplidar_driver_impl.h" -#include "rplidar_driver_serial.h" -#include "rplidar_driver_TCP.h" - -#include - -#ifndef min -#define min(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -namespace rp { namespace standalone{ namespace rplidar { - -#define DEPRECATED_WARN(fn, replacement) do { \ - static bool __shown__ = false; \ - if (!__shown__) { \ - printDeprecationWarn(fn, replacement); \ - __shown__ = true; \ - } \ - } while (0) - - static void printDeprecationWarn(const char* fn, const char* replacement) - { - fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); - } - -static void convert(const rplidar_response_measurement_node_t& from, rplidar_response_measurement_node_hq_t& to) -{ - to.angle_z_q14 = (((from.angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle - to.dist_mm_q2 = from.distance_q2; - to.flag = (from.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field - to.quality = (from.sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 -} - -static void convert(const rplidar_response_measurement_node_hq_t& from, rplidar_response_measurement_node_t& to) -{ - to.sync_quality = (from.flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); - to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); - to.distance_q2 = from.dist_mm_q2 > _u16(-1) ? _u16(0) : _u16(from.dist_mm_q2); -} - -// Factory Impl -RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) -{ - switch (drivertype) { - case DRIVER_TYPE_SERIALPORT: - return new RPlidarDriverSerial(); - case DRIVER_TYPE_TCP: - return new RPlidarDriverTCP(); - default: - return NULL; - } -} - - -void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) -{ - delete drv; -} - - -RPlidarDriverImplCommon::RPlidarDriverImplCommon() - : _isConnected(false) - , _isScanning(false) - , _isSupportingMotorCtrl(false) -{ - _cached_scan_node_hq_count = 0; - _cached_scan_node_hq_count_for_interval_retrieve = 0; - _cached_sampleduration_std = LEGACY_SAMPLE_DURATION; - _cached_sampleduration_express = LEGACY_SAMPLE_DURATION; -} - -bool RPlidarDriverImplCommon::isConnected() -{ - return _isConnected; -} - - -u_result RPlidarDriverImplCommon::reset(_u32 timeout) -{ - u_result ans; - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { - return ans; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::clearNetSerialRxCache() -{ - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _chanDev->flush(); - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_ans_header_t)]; - _u8 *headerBuffer = reinterpret_cast<_u8 *>(header); - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout - waitTime, &recvSize); - if(!ans) return RESULT_OPERATION_TIMEOUT; - - if(recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: - if (currentByte != RPLIDAR_ANS_SYNC_BYTE1) { - continue; - } - - break; - case 1: - if (currentByte != RPLIDAR_ANS_SYNC_BYTE2) { - recvPos = 0; - continue; - } - break; - } - headerBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(rplidar_ans_header_t)) { - return RESULT_OK; - } - } - } - - return RESULT_OPERATION_TIMEOUT; -} - - -u_result RPlidarDriverImplCommon::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) -{ - u_result ans; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_device_health_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); - } - return RESULT_OK; -} - - - -u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) -{ - u_result ans; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(rplidar_response_device_info_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); - if ((info.model >> 4) > RPLIDAR_TOF_MINUM_MAJOR_ID){ - _isTofLidar = true; - }else { - _isTofLidar = false; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkIfTofLidar(bool & isTofLidar, _u32 timeout) -{ - isTofLidar = _isTofLidar; - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) -{ - DEPRECATED_WARN("getFrequency(bool,size_t,float&,bool&)", "getFrequency(const RplidarScanMode&,size_t,float&)"); - - _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std; - frequency = 1000000.0f/(count * sample_duration); - - if (sample_duration <= 277) { - is4kmode = true; - } else { - is4kmode = false; - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) -{ - float sample_duration = scanMode.us_per_sample; - frequency = 1000000.0f / (count * sample_duration); - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)]; - _u8 *nodeBuffer = (_u8*)node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) return RESULT_OPERATION_FAIL; - - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync bit and its reverse in this byte - { - _u8 tmp = (currentByte>>1); - if ( (tmp ^ currentByte) & 0x1 ) { - // pass - } else { - continue; - } - - } - break; - case 1: // expect the highest bit to be 1 - { - if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { - // pass - } else { - recvPos = 0; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(rplidar_response_measurement_node_t)) { - return RESULT_OK; - } - } - } - - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) -{ - if (!_isConnected) { - count = 0; - return RESULT_OPERATION_FAIL; - } - - size_t recvNodeCount = 0; - _u32 startTs = getms(); - _u32 waitTime; - u_result ans; - - while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { - rplidar_response_measurement_node_t node; - if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime))) { - return ans; - } - - nodebuffer[recvNodeCount++] = node; - - if (recvNodeCount == count) return RESULT_OK; - } - count = recvNodeCount; - return RESULT_OPERATION_TIMEOUT; -} - - -u_result RPlidarDriverImplCommon::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - - switch (recvPos) { - case 0: // expect the sync bit 1 - { - _u8 tmp = (currentByte>>4); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { - // pass - } else { - _is_previous_capsuledataRdy = false; - continue; - } - - } - break; - case 1: // expect the sync bit 2 - { - _u8 tmp = (currentByte>>4); - if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { - // pass - } else { - recvPos = 0; - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t)) { - // calc the checksum ... - _u8 checksum = 0; - _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2<<4)); - for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); - cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) - { - checksum ^= nodeBuffer[cpos]; - } - if (recvChecksum == checksum) - { - // only consider vaild if the checksum matches... - if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) - { - // this is the first capsule frame in logic, discard the previous cached data... - _is_previous_capsuledataRdy = false; - return RESULT_OK; - } - return RESULT_OK; - } - _is_previous_capsuledataRdy = false; - return RESULT_INVALID_DATA; - } - } - } - _is_previous_capsuledataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout) -{ - if (!_isConnected) { - return RESULT_OPERATION_FAIL; - } - - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync bit 1 - { - _u8 tmp = (currentByte>>4); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { - // pass - } - else { - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - case 1: // expect the sync bit 2 - { - _u8 tmp = (currentByte>>4); - if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { - // pass - } - else { - recvPos = 0; - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { - // calc the checksum ... - _u8 checksum = 0; - _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); - - for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); - cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos) - { - checksum ^= nodeBuffer[cpos]; - } - - if (recvChecksum == checksum) - { - // only consider vaild if the checksum matches... - if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) - { - // this is the first capsule frame in logic, discard the previous cached data... - _is_previous_capsuledataRdy = false; - return RESULT_OK; - } - return RESULT_OK; - } - _is_previous_capsuledataRdy = false; - return RESULT_INVALID_DATA; - } - } - } - _is_previous_capsuledataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_cacheScanData() -{ - rplidar_response_measurement_node_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete - - while(_isScanning) - { - if (IS_FAIL(ans=_waitScanData(local_buf, count))) { - if (ans != RESULT_OPERATION_TIMEOUT) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } - } - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - - rplidar_response_measurement_node_hq_t nodeHq; - convert(local_buf[pos], nodeHq); - local_scan[scan_count++] = nodeHq; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = nodeHq; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - _isScanning = false; - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::startScanNormal(bool force, _u32 timeout) -{ - u_result ans; - if (!isConnected()) return RESULT_OPERATION_FAIL; - if (_isScanning) return RESULT_ALREADY_DONE; - - stop(); //force the previous operation to stop - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(rplidar_response_measurement_node_t)) { - return RESULT_INVALID_DATA; - } - - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheScanData); - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32 timeout) -{ - DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()"); - - rplidar_response_device_info_t devinfo; - - support = false; - u_result ans = getDeviceInfo(devinfo, timeout); - - if (IS_FAIL(ans)) return ans; - - if (devinfo.firmware_version >= ((0x1<<8) | 17)) { - support = true; - rplidar_response_sample_rate_t sample_rate; - getSampleDuration_uS(sample_rate); - _cached_sampleduration_express = sample_rate.express_sample_duration_us; - _cached_sampleduration_std = sample_rate.std_sample_duration_us; - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_cacheCapsuledScanData() -{ - rplidar_response_capsule_measurement_nodes_t capsule_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete - - - - - while(_isScanning) - { - if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } else { - // current data is invalid, do not use it. - continue; - } - } - switch (_cached_express_flag) - { - case 0: - _capsuleToNormal(capsule_node, local_buf, count); - break; - case 1: - _dense_capsuleToNormal(capsule_node, local_buf, count); - break; - } - // - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - _isScanning = false; - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData() -{ - rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitUltraCapsuledNode(ultra_capsule_node); - - while(_isScanning) - { - if (IS_FAIL(ans=_waitUltraCapsuledNode(ultra_capsule_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } else { - // current data is invalid, do not use it. - continue; - } - } - - _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - - _isScanning = false; - - return RESULT_OK; -} - -void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2); - int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360<<8); - } - - int angleInc_q16 = (diffAngle_q8 << 3); - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) - { - int dist_q2[2]; - int angle_q6[2]; - int syncBit[2]; - - dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); - dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); - - int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4)); - int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4)); - - angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10); - syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; - currentAngle_raw_q16 += angleInc_q16; - - - angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10); - syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; - currentAngle_raw_q16 += angleInc_q16; - - for (int cpos = 0; cpos < 2; ++cpos) { - - if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6); - if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6); - - rplidar_response_measurement_node_hq_t node; - - node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); - node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); - node.quality = dist_q2[cpos] ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.dist_mm_q2 = dist_q2[cpos]; - - nodebuffer[nodeCount++] = node; - } - - } - } - - _cached_previous_capsuledata = capsule; - _is_previous_capsuledataRdy = true; -} - -void RPlidarDriverImplCommon::_dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - const rplidar_response_dense_capsule_measurement_nodes_t *dense_capsule = reinterpret_cast(&capsule); - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); - int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360 << 8); - } - - int angleInc_q16 = (diffAngle_q8 << 8)/40; - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_dense_capsuledata.cabins); ++pos) - { - int dist_q2; - int angle_q6; - int syncBit; - const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); - dist_q2 = dist << 2; - angle_q6 = (currentAngle_raw_q16 >> 10); - syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; - currentAngle_raw_q16 += angleInc_q16; - - if (angle_q6 < 0) angle_q6 += (360 << 6); - if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); - - - - rplidar_response_measurement_node_hq_t node; - - node.angle_z_q14 = _u16((angle_q6 << 8) / 90); - node.flag = (syncBit | ((!syncBit) << 1)); - node.quality = dist_q2 ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.dist_mm_q2 = dist_q2; - - nodebuffer[nodeCount++] = node; - - - } - } - - _cached_previous_dense_capsuledata = *dense_capsule; - _is_previous_capsuledataRdy = true; -} - -u_result RPlidarDriverImplCommon::_cacheHqScanData() -{ - rplidar_response_hq_capsule_measurement_nodes_t hq_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - _waitHqNode(hq_node); - while (_isScanning) { - if (IS_FAIL(ans = _waitHqNode(hq_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } - else { - // current data is invalid, do not use it. - continue; - } - } - - _HqToNormal(hq_node, local_buf, count); - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow - } - } - - } - return RESULT_OK; -} - -//CRC calculate -static _u32 table[256];//crc32_table - -//reflect -static _u32 _bitrev(_u32 input, _u16 bw) -{ - _u16 i; - _u32 var; - var = 0; - for (i = 0; i>= 1; - } - return var; -} - -// crc32_table init -static void _crc32_init(_u32 poly) -{ - _u16 i; - _u16 j; - _u32 c; - - poly = _bitrev(poly, 32); - for (i = 0; i<256; i++){ - c = i; - for (j = 0; j<8; j++){ - if (c & 1) - c = poly ^ (c >> 1); - else - c = c >> 1; - } - table[i] = c; - } -} - -static _u32 _crc32cal(_u32 crc, void* input, _u16 len) -{ - _u16 i; - _u8 index; - _u8* pch; - pch = (unsigned char*)input; - _u8 leftBytes = 4 - len & 0x3; - - for (i = 0; i> 8) ^ table[index]; - pch++; - } - - for (i = 0; i < leftBytes; i++) {//zero padding - index = (unsigned char)(crc^0); - crc = (crc >> 8) ^ table[index]; - } - return crc^0xffffffff; -} - -//crc32cal -static u_result _crc32(_u8 *ptr, _u32 len) { - static _u8 tmp; - if (tmp != 1) { - _crc32_init(0x4C11DB7); - tmp = 1; - } - - return _crc32cal(0xFFFFFFFF, ptr,len); -} - -u_result RPlidarDriverImplCommon::_waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout) -{ - if (!_isConnected) { - return RESULT_OPERATION_FAIL; - } - - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_hq_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync byte - { - _u8 tmp = (currentByte); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC ) { - // pass - } - else { - recvPos = 0; - _is_previous_HqdataRdy = false; - continue; - } - } - break; - case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: - { - - } - break; - case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: - { - - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { - _u32 crcCalc2 = _crc32(nodeBuffer, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); - - if(crcCalc2 == node.crc32){ - _is_previous_HqdataRdy = true; - return RESULT_OK; - } - else { - _is_previous_HqdataRdy = false; - return RESULT_INVALID_DATA; - } - - } - } - } - _is_previous_HqdataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -void RPlidarDriverImplCommon::_HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_HqdataRdy) { - for (size_t pos = 0; pos < _countof(_cached_previous_Hqdata.node_hq); ++pos) - { - nodebuffer[nodeCount++] = node_hq.node_hq[pos]; - } - } - _cached_previous_Hqdata = node_hq; - _is_previous_HqdataRdy = true; - -} -//*******************************************HQ support********************************// - -static _u32 _varbitscale_decode(_u32 scaled, _u32 & scaleLevel) -{ - static const _u32 VBS_SCALED_BASE[] = { - RPLIDAR_VARBITSCALE_X16_DEST_VAL, - RPLIDAR_VARBITSCALE_X8_DEST_VAL, - RPLIDAR_VARBITSCALE_X4_DEST_VAL, - RPLIDAR_VARBITSCALE_X2_DEST_VAL, - 0, - }; - - static const _u32 VBS_SCALED_LVL[] = { - 4, - 3, - 2, - 1, - 0, - }; - - static const _u32 VBS_TARGET_BASE[] = { - (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT), - 0, - }; - - for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) - { - int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); - if (remain >= 0) { - scaleLevel = VBS_SCALED_LVL[i]; - return VBS_TARGET_BASE[i] + (remain << scaleLevel); - } - } - return 0; -} - -void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); - int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360 << 8); - } - - int angleInc_q16 = (diffAngle_q8 << 3) / 3; - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) - { - int dist_q2[3]; - int angle_q6[3]; - int syncBit[3]; - - - _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; - - // unpack ... - int dist_major = (combined_x3 & 0xFFF); - - // signed partical integer, using the magic shift here - // DO NOT TOUCH - - int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); - int dist_predict2 = (((int)combined_x3) >> 22); - - int dist_major2; - - _u32 scalelvl1, scalelvl2; - - // prefetch next ... - if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) - { - dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); - } - else { - dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); - } - - // decode with the var bit scale ... - dist_major = _varbitscale_decode(dist_major, scalelvl1); - dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); - - - int dist_base1 = dist_major; - int dist_base2 = dist_major2; - - if ((!dist_major) && dist_major2) { - dist_base1 = dist_major2; - scalelvl1 = scalelvl2; - } - - - dist_q2[0] = (dist_major << 2); - if ((dist_predict1 == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { - dist_q2[1] = 0; - } else { - dist_predict1 = (dist_predict1 << scalelvl1); - dist_q2[1] = (dist_predict1 + dist_base1) << 2; - - } - - if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { - dist_q2[2] = 0; - } else { - dist_predict2 = (dist_predict2 << scalelvl2); - dist_q2[2] = (dist_predict2 + dist_base2) << 2; - } - - - for (int cpos = 0; cpos < 3; ++cpos) - { - - syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; - - int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); - - if (dist_q2[cpos] >= (50 * 4)) - { - const int k1 = 98361; - const int k2 = int(k1 / dist_q2[cpos]); - - offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; - } - - angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); - currentAngle_raw_q16 += angleInc_q16; - - if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); - if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); - - rplidar_response_measurement_node_hq_t node; - - node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); - node.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); - node.dist_mm_q2 = dist_q2[cpos]; - - nodebuffer[nodeCount++] = node; - } - - } - } - - _cached_previous_ultracapsuledata = capsule; - _is_previous_capsuledataRdy = true; -} - -u_result RPlidarDriverImplCommon::checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs) -{ - u_result ans; - - rplidar_response_device_info_t devinfo; - ans = getDeviceInfo(devinfo, timeoutInMs); - if (IS_FAIL(ans)) return ans; - - // if lidar firmware >= 1.24 - if (devinfo.firmware_version >= ((0x1 << 8) | 24)) { - outSupport = true; - } - return ans; -} - -u_result RPlidarDriverImplCommon::getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve, _u32 timeout) -{ - rplidar_payload_get_scan_conf_t query; - memset(&query, 0, sizeof(query)); - query.type = type; - int sizeVec = reserve.size(); - - int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]); - if (sizeVec > maxLen) sizeVec = maxLen; - - if (sizeVec > 0) - memcpy(query.reserved, &reserve[0], reserve.size()); - - u_result ans; - { - rp::hal::AutoLocker l(_lock); - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_GET_LIDAR_CONF) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(type)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - - std::vector<_u8> dataBuf; - dataBuf.resize(header_size); - _chanDev->recvdata(reinterpret_cast<_u8 *>(&dataBuf[0]), header_size); - - //check if returned type is same as asked type - _u32 replyType = -1; - memcpy(&replyType, &dataBuf[0], sizeof(type)); - if (replyType != type) { - return RESULT_INVALID_DATA; - } - - //copy all the payload into &outputBuf - int payLoadLen = header_size - sizeof(type); - - //do consistency check - if (payLoadLen <= 0) { - return RESULT_INVALID_DATA; - } - //copy all payLoadLen bytes to outputBuf - outputBuf.resize(payLoadLen); - memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen); - } - return ans; -} - -u_result RPlidarDriverImplCommon::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> answer; - bool lidarSupportConfigCmds = false; - ans = checkSupportConfigCommands(lidarSupportConfigCmds); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (lidarSupportConfigCmds) - { - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_TYPICAL, answer, std::vector<_u8>(), timeoutInMs); - if (IS_FAIL(ans)) { - return ans; - } - if (answer.size() < sizeof(_u16)) { - return RESULT_INVALID_DATA; - } - - const _u16 *p_answer = reinterpret_cast(&answer[0]); - outMode = *p_answer; - return ans; - } - //old version of triangle lidar - else - { - outMode = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; - return ans; - } - return ans; -} + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ -u_result RPlidarDriverImplCommon::getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "rplidar_driver.h" +#include "sl_crc.h" +#include - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u32)) - { - return RESULT_INVALID_DATA; - } - const _u32 *result = reinterpret_cast(&answer[0]); - sampleDurationRes = (float)(*result >> 8); - return ans; -} +namespace rp { namespace standalone{ namespace rplidar { -u_result RPlidarDriverImplCommon::getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + RPlidarDriver::RPlidarDriver(){} - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u32)) + RPlidarDriver::RPlidarDriver(sl_u32 channelType) + :_channelType(channelType) { - return RESULT_INVALID_DATA; } - const _u32 *result = reinterpret_cast(&answer[0]); - maxDistance = (float)(*result >> 8); - return ans; -} -u_result RPlidarDriverImplCommon::getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + RPlidarDriver::~RPlidarDriver() {} - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u8)) + RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) { - return RESULT_INVALID_DATA; + //_channelType = drivertype; + return new RPlidarDriver(drivertype); } - const _u8 *result = reinterpret_cast(&answer[0]); - ansType = *result; - return ans; -} -u_result RPlidarDriverImplCommon::getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); - - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) + void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) { - return ans; + delete drv; } - int len = answer.size(); - if (0 == len) return RESULT_INVALID_DATA; - memcpy(modeName, &answer[0], len); - return ans; -} -u_result RPlidarDriverImplCommon::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) -{ - u_result ans; - bool confProtocolSupported = false; - ans = checkSupportConfigCommands(confProtocolSupported); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (confProtocolSupported) - { - // 1. get scan mode count - _u16 modeCount; - ans = getScanModeCount(modeCount); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - // 2. for loop to get all fields of each scan mode - for (_u16 i = 0; i < modeCount; i++) - { - RplidarScanMode scanModeInfoTmp; - memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); - scanModeInfoTmp.id = i; - ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getMaxDistance(scanModeInfoTmp.max_distance, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getScanModeName(scanModeInfoTmp.scan_mode, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - outModes.push_back(scanModeInfoTmp); - } - return ans; - } - else + u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag) { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - //judge if support express scan - bool ifSupportExpScan = false; - ans = checkExpressScanSupported(ifSupportExpScan); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - RplidarScanMode stdScanModeInfo; - stdScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_STD; - stdScanModeInfo.us_per_sample = sampleRateTmp.std_sample_duration_us; - stdScanModeInfo.max_distance = 16; - stdScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; - strcpy(stdScanModeInfo.scan_mode, "Standard"); - outModes.push_back(stdScanModeInfo); - if (ifSupportExpScan) + switch (_channelType) { - RplidarScanMode expScanModeInfo; - expScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; - expScanModeInfo.us_per_sample = sampleRateTmp.express_sample_duration_us; - expScanModeInfo.max_distance = 16; - expScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; - strcpy(expScanModeInfo.scan_mode, "Express"); - outModes.push_back(expScanModeInfo); + case CHANNEL_TYPE_SERIALPORT: + _channel = (*createSerialPortChannel(path, portOrBaud)); + break; + case CHANNEL_TYPE_TCP: + _channel = *createTcpChannel(path, portOrBaud); + break; + case CHANNEL_TYPE_UDP: + _channel = *createUdpChannel(path, portOrBaud); + break; } - return ans; - } - return ans; -} + if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL; + + _lidarDrv = *createLidarDriver(); -u_result RPlidarDriverImplCommon::getScanModeCount(_u16& modeCount, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<_u8>(), timeoutInMs); + if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL; - if (IS_FAIL(ans)) { + sl_result ans =(_lidarDrv)->connect(_channel); return ans; } - if (answer.size() < sizeof(_u16)) { - return RESULT_INVALID_DATA; - } - const _u16 *p_answer = reinterpret_cast(&answer[0]); - modeCount = *p_answer; - - return ans; -} - -u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) -{ - if(_isScanning)return RESULT_ALREADY_DONE; - u_result ans; - - bool ifSupportLidarConf = false; - ans = checkSupportConfigCommands(ifSupportLidarConf); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (useTypicalScan) + void RPlidarDriver::disconnect() { - //if support lidar config protocol - if (ifSupportLidarConf) - { - _u16 typicalMode; - ans = getTypicalScanMode(typicalMode); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - //call startScanExpress to do the job - return startScanExpress(false, typicalMode, 0, outUsedScanMode); - } - //if old version of triangle lidar - else - { - bool isExpScanSupported = false; - ans = checkExpressScanSupported(isExpScanSupported); - if (IS_FAIL(ans)) { - return ans; - } - if (isExpScanSupported) - { - return startScanExpress(false, RPLIDAR_CONF_SCAN_COMMAND_EXPRESS, 0, outUsedScanMode); - } - } + (_lidarDrv)->disconnect(); } - - // 'useTypicalScan' is false, just use normal scan mode - if(ifSupportLidarConf) - { - if(outUsedScanMode) - { - outUsedScanMode->id = RPLIDAR_CONF_SCAN_COMMAND_STD; - ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - } + bool RPlidarDriver::isConnected() + { + return (_lidarDrv)->isConnected(); } - else + + u_result RPlidarDriver::reset(_u32 timeout) { - if(outUsedScanMode) - { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if(IS_FAIL(ans)) return RESULT_INVALID_DATA; - outUsedScanMode->us_per_sample = sampleRateTmp.std_sample_duration_us; - outUsedScanMode->max_distance = 16; - outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; - strcpy(outUsedScanMode->scan_mode, "Standard"); - } + return (_lidarDrv)->reset(); } - return startScanNormal(force); -} - -u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) -{ - u_result ans; - if (!isConnected()) return RESULT_OPERATION_FAIL; - if (_isScanning) return RESULT_ALREADY_DONE; - - stop(); //force the previous operation to stop - - if (scanMode == RPLIDAR_CONF_SCAN_COMMAND_STD) + u_result RPlidarDriver::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) { - return startScan(force, false, 0, outUsedScanMode); + return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs); } - - bool ifSupportLidarConf = false; - ans = checkSupportConfigCommands(ifSupportLidarConf); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (outUsedScanMode) + u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) { - outUsedScanMode->id = scanMode; - if (ifSupportLidarConf) - { - ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - - } - else - { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us; - outUsedScanMode->max_distance = 16; - outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; - strcpy(outUsedScanMode->scan_mode, "Express"); - } + return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs); } - //get scan answer type to specify how to wait data - _u8 scanAnsType; - if (ifSupportLidarConf) - { - getScanModeAnsType(scanAnsType, scanMode); - } - else + u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) { - scanAnsType = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode); } + u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) { - rp::hal::AutoLocker l(_lock); - - rplidar_payload_express_scan_t scanReq; - memset(&scanReq, 0, sizeof(scanReq)); - if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD && scanMode != RPLIDAR_CONF_SCAN_COMMAND_EXPRESS) - scanReq.working_mode = _u8(scanMode); - scanReq.working_flags = options; - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != scanAnsType) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - - if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) - { - if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _cached_express_flag = 0; - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); - } - else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED) - { - if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _cached_express_flag = 1; - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); - } - else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_HQ) { - if (header_size < sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheHqScanData); - } - else - { - if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData); - } - - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; - } + return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout); } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::stop(_u32 timeout) -{ - u_result ans; - _disableDataGrabbing(); - + + u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout) { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) { - return ans; - } + return (_lidarDrv)->getHealth(health, timeout); } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) -{ - DEPRECATED_WARN("grabScanData()", "grabScanDataHq()"); - switch (_dataEvt.wait(timeout)) + u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) { - case rp::hal::Event::EVENT_TIMEOUT: - count = 0; - return RESULT_OPERATION_TIMEOUT; - case rp::hal::Event::EVENT_OK: - { - if(_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout - - rp::hal::AutoLocker l(_lock); - - size_t size_to_copy = min(count, _cached_scan_node_hq_count); - - for (size_t i = 0; i < size_to_copy; i++) - convert(_cached_scan_node_hq_buf[i], nodebuffer[i]); - - count = size_to_copy; - _cached_scan_node_hq_count = 0; - } - return RESULT_OK; - - default: - count = 0; - return RESULT_OPERATION_FAIL; + return (_lidarDrv)->getDeviceInfo(info, timeout); } -} -u_result RPlidarDriverImplCommon::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) -{ - switch (_dataEvt.wait(timeout)) + u_result RPlidarDriver::setMotorPWM(_u16 pwm) { - case rp::hal::Event::EVENT_TIMEOUT: - count = 0; - return RESULT_OPERATION_TIMEOUT; - case rp::hal::Event::EVENT_OK: + return (_lidarDrv)->setMotorSpeed(pwm); + } + + u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout) { - if (_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout - - rp::hal::AutoLocker l(_lock); - - size_t size_to_copy = min(count, _cached_scan_node_hq_count); - memcpy(nodebuffer, _cached_scan_node_hq_buf, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); - - count = size_to_copy; - _cached_scan_node_hq_count = 0; + MotorCtrlSupport motorSupport; + u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout); + if (motorSupport == MotorCtrlSupportNone) + support = false; + return ans; } - return RESULT_OK; - default: - count = 0; - return RESULT_OPERATION_FAIL; - } -} + u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->setLidarIpConf(conf, timeout); + } -u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) -{ - DEPRECATED_WARN("getScanDataWithInterval(rplidar_response_measurement_node_t*, size_t&)", "getScanDataWithInterval(rplidar_response_measurement_node_hq_t*, size_t&)"); + u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs) + { + return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs); + } - size_t size_to_copy = 0; - { - rp::hal::AutoLocker l(_lock); - if(_cached_scan_node_hq_count_for_interval_retrieve == 0) - { - return RESULT_OPERATION_TIMEOUT; - } - //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve - size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; - for (size_t i = 0; i < size_to_copy; i++) - { - convert(_cached_scan_node_hq_buf_for_interval_retrieve[i], nodebuffer[i]); - } - _cached_scan_node_hq_count_for_interval_retrieve = 0; + u_result RPlidarDriver::stop(_u32 timeout) + { + return (_lidarDrv)->stop(timeout); } - count = size_to_copy; - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) -{ - size_t size_to_copy = 0; - // Prevent crash in case lidar is not scanning - that way this function will leave nodebuffer untouched and set - // count to 0. - if (_isScanning) + u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) { - rp::hal::AutoLocker l(_lock); - if (_cached_scan_node_hq_count_for_interval_retrieve == 0) - { - return RESULT_OPERATION_TIMEOUT; - } - // Copy at most count nodes from _cached_scan_node_buf_for_interval_retrieve - size_to_copy = min(_cached_scan_node_hq_count_for_interval_retrieve, count); - memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count_for_interval_retrieve -= size_to_copy; - // Move remaining data to the start of the array. - memmove(&_cached_scan_node_hq_buf_for_interval_retrieve[0], &_cached_scan_node_hq_buf_for_interval_retrieve[size_to_copy], _cached_scan_node_hq_count_for_interval_retrieve * sizeof(rplidar_response_measurement_node_hq_t)); - } - count = size_to_copy; - - // If there is remaining data, return with a warning. - if (_cached_scan_node_hq_count_for_interval_retrieve > 0) - return RESULT_REMAINING_DATA; - return RESULT_OK; -} - -static inline float getAngle(const rplidar_response_measurement_node_t& node) -{ - return (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; -} - -static inline void setAngle(rplidar_response_measurement_node_t& node, float v) -{ - _u16 checkbit = node.angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; - node.angle_q6_checkbit = (((_u16)(v * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; -} - -static inline float getAngle(const rplidar_response_measurement_node_hq_t& node) -{ - return node.angle_z_q14 * 90.f / 16384.f; -} - -static inline void setAngle(rplidar_response_measurement_node_hq_t& node, float v) -{ - node.angle_z_q14 = _u32(v * 16384.f / 90.f); -} - -static inline _u16 getDistanceQ2(const rplidar_response_measurement_node_t& node) -{ - return node.distance_q2; -} - -static inline _u32 getDistanceQ2(const rplidar_response_measurement_node_hq_t& node) -{ - return node.dist_mm_q2; -} - -template -static bool angleLessThan(const TNode& a, const TNode& b) -{ - return getAngle(a) < getAngle(b); -} - -template < class TNode > -static u_result ascendScanData_(TNode * nodebuffer, size_t count) -{ - float inc_origin_angle = 360.f/count; - size_t i = 0; - - //Tune head - for (i = 0; i < count; i++) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - continue; - } else { - while(i != 0) { - i--; - float expect_angle = getAngle(nodebuffer[i+1]) - inc_origin_angle; - if (expect_angle < 0.0f) expect_angle = 0.0f; - setAngle(nodebuffer[i], expect_angle); - } - break; - } - } - - // all the data is invalid - if (i == count) return RESULT_OPERATION_FAIL; - - //Tune tail - for (i = count - 1; i >= 0; i--) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - continue; - } else { - while(i != (count - 1)) { - i++; - float expect_angle = getAngle(nodebuffer[i-1]) + inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - setAngle(nodebuffer[i], expect_angle); - } - break; - } - } - - //Fill invalid angle in the scan - float frontAngle = getAngle(nodebuffer[0]); - for (i = 1; i < count; i++) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - float expect_angle = frontAngle + i * inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - setAngle(nodebuffer[i], expect_angle); - } - } - - // Reorder the scan according to the angle value - std::sort(nodebuffer, nodebuffer + count, &angleLessThan); - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) -{ - DEPRECATED_WARN("ascendScanData(rplidar_response_measurement_node_t*, size_t)", "ascendScanData(rplidar_response_measurement_node_hq_t*, size_t)"); - - return ascendScanData_(nodebuffer, count); -} - -u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) -{ - return ascendScanData_(nodebuffer, count); -} - -u_result RPlidarDriverImplCommon::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) -{ - _u8 pkt_header[10]; - rplidar_cmd_packet_t * header = reinterpret_cast(pkt_header); - _u8 checksum = 0; - - if (!_isConnected) return RESULT_OPERATION_FAIL; - - if (payloadsize && payload) { - cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD; - } - - header->syncByte = RPLIDAR_CMD_SYNC_BYTE; - header->cmd_flag = cmd; - - // send header first - _chanDev->senddata(pkt_header, 2) ; - - if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { - checksum ^= RPLIDAR_CMD_SYNC_BYTE; - checksum ^= cmd; - checksum ^= (payloadsize & 0xFF); - - // calc checksum - for (size_t pos = 0; pos < payloadsize; ++pos) { - checksum ^= ((_u8 *)payload)[pos]; - } - - // send size - _u8 sizebyte = payloadsize; - _chanDev->senddata(&sizebyte, 1); - - // send payload - _chanDev->senddata((const _u8 *)payload, sizebyte); - - // send checksum - _chanDev->senddata(&checksum, 1); - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) -{ - DEPRECATED_WARN("getSampleDuration_uS", "RplidarScanMode::us_per_sample"); - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - rplidar_response_device_info_t devinfo; - // 1. fetch the device version first... - u_result ans = getDeviceInfo(devinfo, timeout); - - rateInfo.express_sample_duration_us = _cached_sampleduration_express; - rateInfo.std_sample_duration_us = _cached_sampleduration_std; - - if (devinfo.firmware_version < ((0x1<<8) | 17)) { - // provide fake data... - - return RESULT_OK; + return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout); } - + u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_sample_rate_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); + return (_lidarDrv)->ascendScanData(nodebuffer, count); } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkMotorCtrlSupport(bool & support, _u32 timeout) -{ - u_result ans; - support = false; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - _disableDataGrabbing(); - + u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) { - rp::hal::AutoLocker l(_lock); - - rplidar_payload_acc_board_flag_t flag; - flag.reserved = 0; - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_acc_board_flag_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - rplidar_response_acc_board_flag_t acc_board_flag; - _chanDev->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(acc_board_flag)); - - if (acc_board_flag.support_flag & RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { - support = true; - } + return RESULT_OPERATION_NOT_SUPPORT; } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) -{ - if (_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; - u_result ans; - rplidar_payload_motor_pwm_t motor_pwm; - motor_pwm.pwm_value = pwm; + u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_SET_MOTOR_PWM,(const _u8 *)&motor_pwm, sizeof(motor_pwm)))) { - return ans; - } - } - - return RESULT_OK; -} - - -u_result RPlidarDriverImplCommon::setLidarSpinSpeed(_u16 rpm, _u32 timeout) -{ - if (!_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; - - u_result ans; - rplidar_payload_hq_spd_ctrl_t speedReq; - speedReq.rpm = rpm; - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL, (const _u8 *)&speedReq, sizeof(speedReq)))) { - return ans; - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::startMotor() -{ - if (!_isTofLidar) { - if (_isSupportingMotorCtrl) { // RPLIDAR A2 - setMotorPWM(DEFAULT_MOTOR_PWM); - delay(500); - return RESULT_OK; - } - else { // RPLIDAR A1 - rp::hal::AutoLocker l(_lock); - _chanDev->clearDTR(); - delay(500); - return RESULT_OK; - } - } - else { - setLidarSpinSpeed(600);//set default rpm to tof lidar - return RESULT_OK; - } - -} - -u_result RPlidarDriverImplCommon::stopMotor() -{ - if(_isTofLidar) return RESULT_OK; - if (_isSupportingMotorCtrl) { // RPLIDAR A2 - setMotorPWM(0); - delay(500); - return RESULT_OK; - } else { // RPLIDAR A1 - rp::hal::AutoLocker l(_lock); - _chanDev->setDTR(); - delay(500); - return RESULT_OK; + return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count); } -} - -void RPlidarDriverImplCommon::_disableDataGrabbing() -{ - _isScanning = false; - _cachethread.join(); -} - -// Serial Driver Impl - -RPlidarDriverSerial::RPlidarDriverSerial() -{ - _chanDev = new SerialChannelDevice(); -} - -RPlidarDriverSerial::~RPlidarDriverSerial() -{ - // force disconnection - disconnect(); - - _chanDev->close(); - _chanDev->ReleaseRxTx(); -} - -void RPlidarDriverSerial::disconnect() -{ - if (!_isConnected) return ; - stop(); -} - -u_result RPlidarDriverSerial::connect(const char * port_path, _u32 baudrate, _u32 flag) -{ - if (isConnected()) return RESULT_ALREADY_DONE; - - if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; + u_result RPlidarDriver::startMotor() { - rp::hal::AutoLocker l(_lock); - - // establish the serial connection... - if (!_chanDev->bind(port_path, baudrate) || !_chanDev->open()) { - return RESULT_INVALID_DATA; - } - _chanDev->flush(); + return (_lidarDrv)->setMotorSpeed(600); } - - _isConnected = true; - - checkMotorCtrlSupport(_isSupportingMotorCtrl); - stopMotor(); - - return RESULT_OK; -} - -RPlidarDriverTCP::RPlidarDriverTCP() -{ - _chanDev = new TCPChannelDevice(); -} - -RPlidarDriverTCP::~RPlidarDriverTCP() -{ - // force disconnection - disconnect(); -} - -void RPlidarDriverTCP::disconnect() -{ - if (!_isConnected) return ; - stop(); - _chanDev->close(); -} - -u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32 flag) -{ - if (isConnected()) return RESULT_ALREADY_DONE; - - if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; - + u_result RPlidarDriver::stopMotor() { - rp::hal::AutoLocker l(_lock); - - // establish the serial connection... - if(!_chanDev->bind(ipStr, port)) - return RESULT_INVALID_DATA; + return (_lidarDrv)->setMotorSpeed(0); } - _isConnected = true; - - checkMotorCtrlSupport(_isSupportingMotorCtrl); - stopMotor(); - - return RESULT_OK; -} - }}} diff --git a/sdk/src/rplidar_driver_TCP.h b/sdk/src/rplidar_driver_TCP.h index 0bd7ffdb..8de2343d 100644 --- a/sdk/src/rplidar_driver_TCP.h +++ b/sdk/src/rplidar_driver_TCP.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/rplidar_driver_impl.h b/sdk/src/rplidar_driver_impl.h index 1cbaca88..75460378 100644 --- a/sdk/src/rplidar_driver_impl.h +++ b/sdk/src/rplidar_driver_impl.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -38,13 +38,10 @@ namespace rp { namespace standalone{ namespace rplidar { class RPlidarDriverImplCommon : public RPlidarDriver { public: - enum { - RPLIDAR_TOF_MINUM_MAJOR_ID = 5, - }; virtual bool isConnected(); virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); - virtual u_result clearNetSerialRxCache(); + virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT); @@ -61,10 +58,8 @@ namespace rp { namespace standalone{ namespace rplidar { virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result setMotorPWM(_u16 pwm); - virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result startMotor(); virtual u_result stopMotor(); virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); @@ -92,21 +87,17 @@ namespace rp { namespace standalone{ namespace rplidar { virtual u_result _cacheCapsuledScanData(); virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - virtual void _dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); //FW1.23 virtual u_result _cacheUltraCapsuledScanData(); virtual u_result _waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); virtual void _ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - virtual u_result _cacheHqScanData(); - virtual u_result _waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); - virtual void _HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); bool _isConnected; bool _isScanning; bool _isSupportingMotorCtrl; - bool _isTofLidar; + rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; size_t _cached_scan_node_hq_count; @@ -115,16 +106,10 @@ namespace rp { namespace standalone{ namespace rplidar { _u16 _cached_sampleduration_std; _u16 _cached_sampleduration_express; - _u8 _cached_express_flag; rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; - rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; - rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; bool _is_previous_capsuledataRdy; - bool _is_previous_HqdataRdy; - - rp::hal::Locker _lock; rp::hal::Event _dataEvt; @@ -134,4 +119,4 @@ namespace rp { namespace standalone{ namespace rplidar { RPlidarDriverImplCommon(); virtual ~RPlidarDriverImplCommon() {} }; -}}} +}}} \ No newline at end of file diff --git a/sdk/src/rplidar_driver_serial.h b/sdk/src/rplidar_driver_serial.h index abc1468a..5ec017e4 100644 --- a/sdk/src/rplidar_driver_serial.h +++ b/sdk/src/rplidar_driver_serial.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/sdk/src/sdkcommon.h b/sdk/src/sdkcommon.h index d025dfb1..f928b42a 100644 --- a/sdk/src/sdkcommon.h +++ b/sdk/src/sdkcommon.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,7 +33,8 @@ */ #if defined(_WIN32) -#include "arch\win32\arch_win32.h" + +#include "arch/win32/arch_win32.h" #elif defined(_MACOS) #include "arch/macOS/arch_macOS.h" #elif defined(__GNUC__) diff --git a/sdk/src/sl_crc.cpp b/sdk/src/sl_crc.cpp new file mode 100644 index 00000000..275222fa --- /dev/null +++ b/sdk/src/sl_crc.cpp @@ -0,0 +1,102 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_crc.h" + +namespace sl {namespace crc32 { + + static sl_u32 table[256];//crc32_table + sl_u32 bitrev(sl_u32 input, sl_u16 bw) + { + sl_u16 i; + sl_u32 var; + var = 0; + for (i = 0; i < bw; i++) { + if (input & 0x01) { + var |= 1 << (bw - 1 - i); + } + input >>= 1; + } + return var; + } + + void init(sl_u32 poly) + { + sl_u16 i; + sl_u16 j; + sl_u32 c; + + poly = bitrev(poly, 32); + for (i = 0; i < 256; i++) { + c = i; + for (j = 0; j < 8; j++) { + if (c & 1) + c = poly ^ (c >> 1); + else + c = c >> 1; + } + table[i] = c; + } + } + + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len) + { + sl_u16 i; + sl_u8 index; + sl_u8* pch; + pch = (unsigned char*)input; + sl_u8 leftBytes = 4 - len & 0x3; + + for (i = 0; i < len; i++) { + index = (unsigned char)(crc^*pch); + crc = (crc >> 8) ^ table[index]; + pch++; + } + + for (i = 0; i < leftBytes; i++) {//zero padding + index = (unsigned char)(crc ^ 0); + crc = (crc >> 8) ^ table[index]; + } + return crc ^ 0xffffffff; + } + + sl_result getResult(sl_u8 *ptr, sl_u32 len) + { + static sl_u8 tmp; + if (tmp != 1) { + init(0x4C11DB7); + tmp = 1; + } + + return cal(0xFFFFFFFF, ptr, len); + } +}} \ No newline at end of file diff --git a/sdk/src/sl_lidar_driver.cpp b/sdk/src/sl_lidar_driver.cpp new file mode 100644 index 00000000..1e7e234c --- /dev/null +++ b/sdk/src/sl_lidar_driver.cpp @@ -0,0 +1,1987 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include + +#ifdef _WIN32 +#define NOMINMAX +#undef min +#undef max +#endif + +#if defined(__cplusplus) && __cplusplus >= 201103L +#ifndef _GXX_NULLPTR_T +#define _GXX_NULLPTR_T +typedef decltype(nullptr) nullptr_t; +#endif +#endif /* C++11. */ + +namespace sl { + static void printDeprecationWarn(const char* fn, const char* replacement) + { + fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); + } + + static void convert(const sl_lidar_response_measurement_node_t& from, sl_lidar_response_measurement_node_hq_t& to) + { + to.angle_z_q14 = (((from.angle_q6_checkbit) >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + to.dist_mm_q2 = from.distance_q2; + to.flag = (from.sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + to.quality = (from.sync_quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 + } + + static void convert(const sl_lidar_response_measurement_node_hq_t& from, sl_lidar_response_measurement_node_t& to) + { + to.sync_quality = (from.flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); + to.distance_q2 = from.dist_mm_q2 > sl_u16(-1) ? sl_u16(0) : sl_u16(from.dist_mm_q2); + } + + static sl_u32 _varbitscale_decode(sl_u32 scaled, sl_u32 & scaleLevel) + { + static const sl_u32 VBS_SCALED_BASE[] = { + SL_LIDAR_VARBITSCALE_X16_DEST_VAL, + SL_LIDAR_VARBITSCALE_X8_DEST_VAL, + SL_LIDAR_VARBITSCALE_X4_DEST_VAL, + SL_LIDAR_VARBITSCALE_X2_DEST_VAL, + 0, + }; + + static const sl_u32 VBS_SCALED_LVL[] = { + 4, + 3, + 2, + 1, + 0, + }; + + static const sl_u32 VBS_TARGET_BASE[] = { + (0x1 << SL_LIDAR_VARBITSCALE_X16_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X8_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X4_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X2_SRC_BIT), + 0, + }; + + for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) { + int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); + if (remain >= 0) { + scaleLevel = VBS_SCALED_LVL[i]; + return VBS_TARGET_BASE[i] + (remain << scaleLevel); + } + } + return 0; + } + + static inline float getAngle(const sl_lidar_response_measurement_node_t& node) + { + return (node.angle_q6_checkbit >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_t& node, float v) + { + sl_u16 checkbit = node.angle_q6_checkbit & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT; + node.angle_q6_checkbit = (((sl_u16)(v * 64.0f)) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; + } + + static inline float getAngle(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.angle_z_q14 * 90.f / 16384.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_hq_t& node, float v) + { + node.angle_z_q14 = sl_u32(v * 16384.f / 90.f); + } + + static inline sl_u16 getDistanceQ2(const sl_lidar_response_measurement_node_t& node) + { + return node.distance_q2; + } + + static inline sl_u32 getDistanceQ2(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.dist_mm_q2; + } + + template + static bool angleLessThan(const TNode& a, const TNode& b) + { + return getAngle(a) < getAngle(b); + } + + template < class TNode > + static sl_result ascendScanData_(TNode * nodebuffer, size_t count) + { + float inc_origin_angle = 360.f / count; + size_t i = 0; + + //Tune head + for (i = 0; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != 0) { + i--; + float expect_angle = getAngle(nodebuffer[i + 1]) - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + // all the data is invalid + if (i == count) return SL_RESULT_OPERATION_FAIL; + + //Tune tail + for (i = count - 1; i >= 0; i--) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != (count - 1)) { + i++; + float expect_angle = getAngle(nodebuffer[i - 1]) + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + //Fill invalid angle in the scan + float frontAngle = getAngle(nodebuffer[0]); + for (i = 1; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + } + + // Reorder the scan according to the angle value + std::sort(nodebuffer, nodebuffer + count, &angleLessThan); + + return SL_RESULT_OK; + } + + class SlamtecLidarDriver :public ILidarDriver + { + public: + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + + enum { + NORMAL_CAPSULE = 0, + DENSE_CAPSULE = 1, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + TOF_LIDAR_MINUM_MAJOR_ID = 6, + }; + + public: + SlamtecLidarDriver() + : _channel(NULL) + , _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _cached_sampleduration_std(LEGACY_SAMPLE_DURATION) + , _cached_sampleduration_express(LEGACY_SAMPLE_DURATION) + , _cached_scan_node_hq_count(0) + , _cached_scan_node_hq_count_for_interval_retrieve(0) + {} + + sl_result connect(IChannel* channel) + { + Result ans = SL_RESULT_OK; + + if (!channel) return SL_RESULT_OPERATION_FAIL; + if (isConnected()) return SL_RESULT_ALREADY_DONE; + _channel = channel; + + { + rp::hal::AutoLocker l(_lock); + ans = _channel->open(); + if (!ans) + return SL_RESULT_OPERATION_FAIL; + + _channel->flush(); + } + + _isConnected = true; + + ans =checkMotorCtrlSupport(_isSupportingMotorCtrl,500); + return SL_RESULT_OK; + + } + + void disconnect() + { + if (_isConnected) + _channel->close(); + } + + bool isConnected() + { + return _isConnected; + } + + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_RESET); + if (!ans) { + return ans; + } + } + return SL_RESULT_OK; + } + + sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + bool confProtocolSupported = false; + ans = checkSupportConfigCommands(confProtocolSupported); + if (!ans) return SL_RESULT_INVALID_DATA; + + if (confProtocolSupported) { + // 1. get scan mode count + sl_u16 modeCount; + ans = getScanModeCount(modeCount); + if (!ans) return ans; + // 2. for loop to get all fields of each scan mode + for (sl_u16 i = 0; i < modeCount; i++) { + LidarScanMode scanModeInfoTmp; + memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); + scanModeInfoTmp.id = i; + ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i); + if (!ans) return ans; + ans = getMaxDistance(scanModeInfoTmp.max_distance, i); + if (!ans) return ans; + ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i); + if (!ans) return ans; + ans = getScanModeName(scanModeInfoTmp.scan_mode, i); + if (!ans) return ans; + outModes.push_back(scanModeInfoTmp); + + } + return ans; + } + + return ans; + } + + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector answer; + bool lidarSupportConfigCmds = false; + ans = checkSupportConfigCommands(lidarSupportConfigCmds); + if (!ans) return ans; + + if (lidarSupportConfigCmds) { + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_TYPICAL, answer, std::vector(), timeoutInMs); + if (!ans) return ans; + if (answer.size() < sizeof(sl_u16)) { + return SL_RESULT_INVALID_DATA; + } + const sl_u16 *p_answer = reinterpret_cast(&answer[0]); + outMode = *p_answer; + return ans; + } + //old version of triangle lidar + else { + outMode = SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS; + return ans; + } + return ans; + + } + + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) + { + Result ans = SL_RESULT_OK; + bool ifSupportLidarConf = false; + startMotor(); + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return ans; + if (useTypicalScan){ + sl_u16 typicalMode; + ans = getTypicalScanMode(typicalMode); + if (!ans) return ans; + + //call startScanExpress to do the job + return startScanExpress(false, typicalMode, 0, outUsedScanMode); + } + + // 'useTypicalScan' is false, just use normal scan mode + if (ifSupportLidarConf) { + if (outUsedScanMode) { + outUsedScanMode->id = SL_LIDAR_CONF_SCAN_COMMAND_STD; + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (!ans) return ans; + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (!ans) return ans; + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (!ans) return ans; + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if (!ans) return ans; + } + } + + return startScanNormal(force); + } + + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + if (_isScanning) return SL_RESULT_ALREADY_DONE; + + stop(); //force the previous operation to stop + setMotorSpeed(); + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(force ? SL_LIDAR_CMD_FORCE_SCAN : SL_LIDAR_CMD_SCAN); + if (!ans) return ans; + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) return ans; + + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_MEASUREMENT) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(sl_lidar_response_measurement_node_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheScanData); + if (_cachethread.getHandle() == 0) { + return SL_RESULT_OPERATION_FAIL; + } + } + return SL_RESULT_OK; + } + + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + if (_isScanning) return SL_RESULT_ALREADY_DONE; + stop(); //force the previous operation to stop + + bool ifSupportLidarConf = false; + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return SL_RESULT_INVALID_DATA; + + + if (outUsedScanMode) { + outUsedScanMode->id = scanMode; + if (ifSupportLidarConf) { + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + + } + + } + + //get scan answer type to specify how to wait data + sl_u8 scanAnsType; + if (ifSupportLidarConf) { + getScanModeAnsType(scanAnsType, scanMode); + } + else { + scanAnsType = SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + } + if (!ifSupportLidarConf || scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT) { + if (scanMode == SL_LIDAR_CONF_SCAN_COMMAND_STD) { + return startScan(force, false, 0, outUsedScanMode); + } + } + { + rp::hal::AutoLocker l(_lock); + + startMotor(); + sl_lidar_payload_express_scan_t scanReq; + memset(&scanReq, 0, sizeof(scanReq)); + if (!ifSupportLidarConf){ + if (scanMode != SL_LIDAR_CONF_SCAN_COMMAND_STD && scanMode != SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS) + scanReq.working_mode = sl_u8(scanMode); + }else + scanReq.working_mode = sl_u8(scanMode); + + scanReq.working_flags = options; + delay(5); + ans = _sendCommand(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)); + if (!ans) { + ans = _sendCommand(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)); + if (!ans) + return SL_RESULT_INVALID_DATA; + } + + + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) return ans; + + // verify whether we got a correct header + if (response_header.type != scanAnsType) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + + if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) { + if (header_size < sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _cached_capsule_flag = NORMAL_CAPSULE; + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheCapsuledScanData); + } + else if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED) { + if (header_size < sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _cached_capsule_flag = DENSE_CAPSULE; + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheCapsuledScanData); + } + else if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ) { + if (header_size < sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheHqScanData); + } + else { + if (header_size < sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheUltraCapsuledScanData); + } + + if (_cachethread.getHandle() == 0) { + return SL_RESULT_OPERATION_FAIL; + } + } + return SL_RESULT_OK; + + } + + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_STOP); + if (!ans) return ans; + } + delay(100); + + if(_isSupportingMotorCtrl == MotorCtrlSupportPwm) + setMotorSpeed(0); + + return SL_RESULT_OK; + } + + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) + { + switch (_dataEvt.wait(timeout)) + { + case rp::hal::Event::EVENT_TIMEOUT: + count = 0; + return SL_RESULT_OPERATION_TIMEOUT; + case rp::hal::Event::EVENT_OK: + { + if (_cached_scan_node_hq_count == 0) return SL_RESULT_OPERATION_TIMEOUT; //consider as timeout + + rp::hal::AutoLocker l(_lock); + + size_t size_to_copy = std::min(count, _cached_scan_node_hq_count); + memcpy(nodebuffer, _cached_scan_node_hq_buf, size_to_copy * sizeof(sl_lidar_response_measurement_node_hq_t)); + + count = size_to_copy; + _cached_scan_node_hq_count = 0; + } + return SL_RESULT_OK; + + default: + count = 0; + return SL_RESULT_OPERATION_FAIL; + } + } + + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + _disableDataGrabbing(); + delay(20); + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_DEVICE_INFO); + if (!ans) return ans; + return _waitResponse(info, SL_LIDAR_ANS_TYPE_DEVINFO); + } + } + + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + support = MotorCtrlSupportNone; + _disableDataGrabbing(); + { + sl_lidar_response_device_info_t devInfo; + ans = getDeviceInfo(devInfo, 500); + if (!ans) return ans; + sl_u8 majorId = devInfo.model >> 4; + if (majorId >= TOF_LIDAR_MINUM_MAJOR_ID) { + support = MotorCtrlSupportRpm; + return ans; + } + else if(majorId >= A2A3_LIDAR_MINUM_MAJOR_ID){ + + rp::hal::AutoLocker l(_lock); + sl_lidar_payload_acc_board_flag_t flag; + flag.reserved = 0; + ans = _sendCommand(SL_LIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)); + if (!ans) return ans; + + sl_lidar_response_acc_board_flag_t acc_board_flag; + ans = _waitResponse(acc_board_flag, SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG); + if (acc_board_flag.support_flag & SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { + support = MotorCtrlSupportPwm; + } + return ans; + } + + } + return SL_RESULT_OK; + + } + + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) + { + float sample_duration = scanMode.us_per_sample; + frequency = 1000000.0f / (count * sample_duration); + return SL_RESULT_OK; + } + + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout) + { + sl_result ans = setLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, &conf, sizeof(sl_lidar_ip_conf_t), timeout); + return ans; + } + + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + + if (!isConnected()) + return SL_RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_DEVICE_HEALTH); + if (!ans) return ans; + delay(50); + ans = _waitResponse(health, SL_LIDAR_ANS_TYPE_DEVHEALTH); + + } + return ans; + + } + + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs) + { + Result ans = SL_RESULT_OK; + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_LIDAR_MAC_ADDR, answer, std::vector(), timeoutInMs); + if (!ans) return ans; + + int len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(macAddrArray, &answer[0], len); + return ans; + } + + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return ascendScanData_(nodebuffer, count); + } + + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + size_t size_to_copy = 0; + { + rp::hal::AutoLocker l(_lock); + if (_cached_scan_node_hq_count_for_interval_retrieve == 0) { + return SL_RESULT_OPERATION_TIMEOUT; + } + //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve + size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; + memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count_for_interval_retrieve = 0; + } + count = size_to_copy; + + return SL_RESULT_OK; + } + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) + { + Result ans = SL_RESULT_OK; + + if(speed == DEFAULT_MOTOR_SPEED){ + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (!ans) return ans; + if(_isSupportingMotorCtrl == MotorCtrlSupportPwm) + speed = desired_speed.pwm_ref; + else + speed = desired_speed.rpm; + } + switch (_isSupportingMotorCtrl) + { + case MotorCtrlSupportNone: + break; + case MotorCtrlSupportPwm: + sl_lidar_payload_motor_pwm_t motor_pwm; + motor_pwm.pwm_value = speed; + ans = _sendCommand(SL_LIDAR_CMD_SET_MOTOR_PWM, (const sl_u8 *)&motor_pwm, sizeof(motor_pwm)); + if (!ans) return ans; + break; + case MotorCtrlSupportRpm: + sl_lidar_payload_motor_pwm_t motor_rpm; + motor_rpm.pwm_value = speed; + ans = _sendCommand(SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL, (const sl_u8 *)&motor_rpm, sizeof(motor_rpm)); + if (!ans) return ans; + break; + } + return SL_RESULT_OK; + } + + sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs) + { + Result ans = SL_RESULT_OK; + rp::hal::AutoLocker l(_lock); + { + std::vector answer; + + ans = getLidarConf(RPLIDAR_CONF_MIN_ROT_FREQ, answer, std::vector()); + if (!ans) return ans; + + const sl_u16 *min_answer = reinterpret_cast(&answer[0]); + motorInfo.min_speed = *min_answer; + + + ans = getLidarConf(RPLIDAR_CONF_MAX_ROT_FREQ, answer, std::vector()); + if (!ans) return ans; + + const sl_u16 *max_answer = reinterpret_cast(&answer[0]); + motorInfo.max_speed = *max_answer; + + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (!ans) return ans; + if(motorInfo.motorCtrlSupport == MotorCtrlSupportPwm) + motorInfo.desired_speed = desired_speed.pwm_ref; + else + motorInfo.desired_speed = desired_speed.rpm; + + } + return SL_RESULT_OK; + } + + protected: + sl_result startMotor() + { + return setMotorSpeed(600); + } + + sl_result getDesiredSpeed(sl_lidar_response_desired_rot_speed_t & motorSpeed, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_DESIRED_ROT_FREQ, answer, std::vector(), timeoutInMs); + + if (!ans) return ans; + + const sl_lidar_response_desired_rot_speed_t *p_answer = reinterpret_cast(&answer[0]); + motorSpeed = *p_answer; + return SL_RESULT_OK; + } + + sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + sl_lidar_response_device_info_t devinfo; + ans = getDeviceInfo(devinfo, timeoutInMs); + if (!ans) return ans; + + sl_u16 modecount; + ans = getScanModeCount(modecount, 250); + if ((sl_result)ans == SL_RESULT_OK) + outSupport = true; + + return SL_RESULT_OK; + } + + sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector(), timeoutInMs); + + if (!ans) return ans; + + const sl_u16 *p_answer = reinterpret_cast(&answer[0]); + modeCount = *p_answer; + return SL_RESULT_OK; + } + + sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout) + { + if (type < 0x00010000 || type >0x0001FFFF) + return SL_RESULT_INVALID_DATA; + std::vector requestPkt; + requestPkt.resize(sizeof(sl_lidar_payload_set_scan_conf_t) + payloadSize); + if (!payload) payloadSize = 0; + sl_lidar_payload_set_scan_conf_t* query = reinterpret_cast(&requestPkt[0]); + + query->type = type; + + if (payloadSize) + memcpy(&query[1], payload, payloadSize); + + sl_result ans; + { + rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(SL_LIDAR_CMD_SET_LIDAR_CONF, &requestPkt[0], requestPkt.size()))) {// + return ans; + } + delay(20); + // waiting for confirmation + sl_lidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF) { + return SL_RESULT_INVALID_DATA; + } + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(type)) { + return SL_RESULT_INVALID_DATA; + } + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + delay(100); + struct _sl_lidar_response_set_lidar_conf { + sl_u32 type; + sl_u32 result; + } answer; + + _channel->read(reinterpret_cast(&answer), header_size); + return answer.result; + + } + + } + + sl_result getLidarConf(sl_u32 type, std::vector &outputBuf, const std::vector &reserve = std::vector(), sl_u32 timeout = DEFAULT_TIMEOUT) + { + sl_lidar_payload_get_scan_conf_t query; + query.type = type; + int sizeVec = reserve.size(); + + int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]); + if (sizeVec > maxLen) sizeVec = maxLen; + + if (sizeVec > 0) + memcpy(query.reserved, &reserve[0], reserve.size()); + + Result ans = SL_RESULT_OK; + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)); + if (!ans) return ans; + //delay(50); + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans)return ans; + + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(type)) { + return SL_RESULT_INVALID_DATA; + } + //delay(100); + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + + std::vector dataBuf; + dataBuf.resize(header_size); + _channel->read(reinterpret_cast(&dataBuf[0]), header_size); + + //check if returned type is same as asked type + sl_u32 replyType = -1; + memcpy(&replyType, &dataBuf[0], sizeof(type)); + if (replyType != type) { + return SL_RESULT_INVALID_DATA; + } + + //copy all the payload into &outputBuf + int payLoadLen = header_size - sizeof(type); + + //do consistency check + if (payLoadLen <= 0) { + return SL_RESULT_INVALID_DATA; + } + //copy all payLoadLen bytes to outputBuf + outputBuf.resize(payLoadLen); + memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen); + + } + + return SL_RESULT_OK; + } + + sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs); + + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u32)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u32 *result = reinterpret_cast(&answer[0]); + sampleDurationRes = (float)(*result >> 8); + return ans; + } + + sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs); + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u32)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u32 *result = reinterpret_cast(&answer[0]); + maxDistance = (float)(*result >> 8); + return ans; + } + + sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs); + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u8)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u8 *result = reinterpret_cast(&answer[0]); + ansType = *result; + return ans; + + } + + sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result ans = SL_RESULT_OK; + std::vector reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs); + if (!ans) return ans; + int len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(modeName, &answer[0], len); + return ans; + } + + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32 * baudRateDetected) + { + // ask the LIDAR to stop working first... + stop(); + _channel->flush(); + + // wait for a while + delay(10); + _channel->clearReadCache(); + + // sending magic byte to let the target LIDAR start baudrate measurement + // More than 100 bytes per second datarate is required to trigger the measurements + { + + + sl_u8 magicByteSeq[16]; + + memset(magicByteSeq, SL_LIDAR_AUTOBAUD_MAGICBYTE, sizeof(magicByteSeq)); + + sl_u64 startTS = getms(); + + while (getms() - startTS < 1500 ) //lasting for 1.5sec + { + if (_channel->write(magicByteSeq, sizeof(magicByteSeq)) < 0) + { + return SL_RESULT_OPERATION_FAIL; + } + + size_t dataCountGot; + if (_channel->waitForData(1, 1, &dataCountGot)) { + //got reply, stop + break; + } + } + } + + // getback the bps measured + _u32 bpsDetected = 0; + size_t dataCountGot; + if (_channel->waitForData(4, 500, &dataCountGot)) { + //got reply, stop + _channel->read(&bpsDetected, 4); + if (baudRateDetected) *baudRateDetected = bpsDetected; + + + // send a confirmation to the LIDAR, otherwise, the previous baudrate will be reverted back + sl_lidar_payload_new_bps_confirmation_t confirmation; + confirmation.flag = 0x5F5F; + confirmation.required_bps = requiredBaudRate; + confirmation.param = 0; + if (SL_IS_FAIL(_sendCommand(RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM, &confirmation, sizeof(confirmation)))) + return RESULT_OPERATION_FAIL; + + + return RESULT_OK; + } + + return RESULT_OPERATION_TIMEOUT; + } + + private: + + sl_result _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 ) + { + sl_u8 pkt_header[10]; + sl_u8 checksum = 0; + + std::vector cmd_packet; + cmd_packet.clear(); + + if (payloadsize && payload) { + cmd |= SL_LIDAR_CMDFLAG_HAS_PAYLOAD; + } + _channel->flush(); + cmd_packet.push_back(SL_LIDAR_CMD_SYNC_BYTE); + cmd_packet.push_back(cmd); + + if (cmd & SL_LIDAR_CMDFLAG_HAS_PAYLOAD) { + std::vector payloadMsg; + checksum ^= SL_LIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + // send size + sl_u8 sizebyte = payloadsize; + cmd_packet.push_back(sizebyte); + // calc checksum + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((sl_u8 *)payload)[pos]; + cmd_packet.push_back(((sl_u8 *)payload)[pos]); + } + cmd_packet.push_back(checksum); + + } + sl_u8 packet[1024]; + for (int pos = 0; pos < cmd_packet.size(); pos++) { + packet[pos] = cmd_packet[pos]; + } + _channel->write(packet, cmd_packet.size()); + delay(1); + return SL_RESULT_OK; + } + + sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_ans_header_t)]; + sl_u8 *headerBuffer = reinterpret_cast(header); + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_ans_header_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_TIMEOUT; + + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != SL_LIDAR_ANS_SYNC_BYTE1) { + continue; + } + + break; + case 1: + if (currentByte != SL_LIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(sl_lidar_ans_header_t)) { + return SL_RESULT_OK; + } + } + } + + return SL_RESULT_OPERATION_TIMEOUT; + } + + template + sl_result _waitResponse(T &payload ,sl_u8 ansType, sl_u32 timeout = DEFAULT_TIMEOUT) + { + sl_lidar_ans_header_t response_header; + Result ans = SL_RESULT_OK; + //delay(100); + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) + return ans; + // verify whether we got a correct header + if (response_header.type != ansType) { + return SL_RESULT_INVALID_DATA; + } + // delay(50); + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(T)) { + return SL_RESULT_INVALID_DATA; + } + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + _channel->read(reinterpret_cast(&payload), sizeof(T)); + return SL_RESULT_OK; + } + + void _disableDataGrabbing() + { + //_clearRxDataCache(); + _isScanning = false; + _cachethread.join(); + } + +#define MAX_SCAN_NODES (8192) + sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_measurement_node_t)]; + sl_u8 *nodeBuffer = (sl_u8*)node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_measurement_node_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_FAIL; + + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit and its reverse in this byte + { + sl_u8 tmp = (currentByte >> 1); + if ((tmp ^ currentByte) & 0x1) { + // pass + } + else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (currentByte & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } + else { + recvPos = 0; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(sl_lidar_response_measurement_node_t)) { + return SL_RESULT_OK; + } + } + } + + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + count = 0; + return SL_RESULT_OPERATION_FAIL; + } + + size_t recvNodeCount = 0; + sl_u32 startTs = getms(); + sl_u32 waitTime; + Result ans = SL_RESULT_OK; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + sl_lidar_response_measurement_node_t node; + ans = _waitNode(&node, timeout - waitTime); + if (!ans) return ans; + + nodebuffer[recvNodeCount++] = node; + + if (recvNodeCount == count) return SL_RESULT_OK; + } + count = recvNodeCount; + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _cacheScanData() + { + + sl_lidar_response_measurement_node_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete + + while (_isScanning) { + ans = _waitScanData(local_buf, count); + + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + } + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + + sl_lidar_response_measurement_node_hq_t nodeHq; + convert(local_buf[pos], nodeHq); + local_scan[scan_count++] = nodeHq; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = nodeHq; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + _isScanning = false; + return SL_RESULT_OK; + } + + void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3) / 3; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) { + int dist_q2[3]; + int angle_q6[3]; + int syncBit[3]; + + + sl_u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; + + // unpack ... + int dist_major = (combined_x3 & 0xFFF); + + // signed partical integer, using the magic shift here + // DO NOT TOUCH + + int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); + int dist_predict2 = (((int)combined_x3) >> 22); + + int dist_major2; + + sl_u32 scalelvl1, scalelvl2; + + // prefetch next ... + if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) { + dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); + } + else { + dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); + } + + // decode with the var bit scale ... + dist_major = _varbitscale_decode(dist_major, scalelvl1); + dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); + + + int dist_base1 = dist_major; + int dist_base2 = dist_major2; + + if ((!dist_major) && dist_major2) { + dist_base1 = dist_major2; + scalelvl1 = scalelvl2; + } + + + dist_q2[0] = (dist_major << 2); + if ((dist_predict1 == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { + dist_q2[1] = 0; + } + else { + dist_predict1 = (dist_predict1 << scalelvl1); + dist_q2[1] = (dist_predict1 + dist_base1) << 2; + + } + + if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { + dist_q2[2] = 0; + } + else { + dist_predict2 = (dist_predict2 << scalelvl2); + dist_q2[2] = (dist_predict2 + dist_base2) << 2; + } + + + for (int cpos = 0; cpos < 3; ++cpos) { + syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + + int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + if (dist_q2[cpos] >= (50 * 4)) + { + const int k1 = 98361; + const int k2 = int(k1 / dist_q2[cpos]); + + offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; + } + + angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + sl_lidar_response_measurement_node_hq_t node; + + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2F << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.angle_z_q14 = sl_u16((angle_q6[cpos] << 8) / 90); + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; + } + + sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_TIMEOUT; + + if (recvSize > remainSize) recvSize = remainSize; + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: // expect the sync bit 1 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + // calc the checksum ... + sl_u8 checksum = 0; + sl_u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); + for (size_t cpos = offsetof(sl_lidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(sl_lidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + if (recvChecksum == checksum) { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) { + // this is the first capsule frame in logic, discard the previous cached data... + _scan_node_synced = false; + _is_previous_capsuledataRdy = false; + return SL_RESULT_OK; + } + return SL_RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3) << 4)); + int angle_offset2_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3) << 4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10); + syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10); + syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + sl_lidar_response_measurement_node_hq_t node; + + node.angle_z_q14 = sl_u16((angle_q6[cpos] << 8) / 90); + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2f << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; + } + + void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + static int lastNodeSyncBit = 0; + const sl_lidar_response_dense_capsule_measurement_nodes_t *dense_capsule = reinterpret_cast(&capsule); + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 8) / 40; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_dense_capsuledata.cabins); ++pos) { + int dist_q2; + int angle_q6; + int syncBit; + const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); + dist_q2 = dist << 2; + angle_q6 = (currentAngle_raw_q16 >> 10); + + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16<<1)) ? 1 : 0; + syncBit = (syncBit^ lastNodeSyncBit)&syncBit;//Ensure that syncBit is exactly detected + if (syncBit) { + _scan_node_synced = true; + } + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + + sl_lidar_response_measurement_node_hq_t node; + + node.angle_z_q14 = sl_u16((angle_q6 << 8) / 90); + node.flag = (syncBit | ((!syncBit) << 1)); + node.quality = dist_q2 ? (0x2f << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.dist_mm_q2 = dist_q2; + if(_scan_node_synced) + nodebuffer[nodeCount++] = node; + lastNodeSyncBit = syncBit; + } + } + else { + _scan_node_synced = false; + } + + _cached_previous_dense_capsuledata = *dense_capsule; + _is_previous_capsuledataRdy = true; + } + + sl_result _cacheCapsuledScanData() + { + sl_lidar_response_capsule_measurement_nodes_t capsule_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete + + while (_isScanning) { + ans = _waitCapsuledNode(capsule_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + switch (_cached_capsule_flag) { + case NORMAL_CAPSULE: + _capsuleToNormal(capsule_node, local_buf, count); + break; + case DENSE_CAPSULE: + _dense_capsuleToNormal(capsule_node, local_buf, count); + break; + } + // + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + _isScanning = false; + + return SL_RESULT_OK; + } + + sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + return SL_RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans){ + return SL_RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync byte + { + sl_u8 tmp = (currentByte); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC) { + // pass + } + else { + recvPos = 0; + _is_previous_HqdataRdy = false; + continue; + } + } + break; + case sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: + { + + } + break; + case sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 1: + { + + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)) { + sl_u32 crcCalc2 = crc32::getResult(nodeBuffer, sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4); + + if (crcCalc2 == node.crc32) { + _is_previous_HqdataRdy = true; + return SL_RESULT_OK; + } + else { + _is_previous_HqdataRdy = false; + return SL_RESULT_INVALID_DATA; + } + + } + } + } + _is_previous_HqdataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + + void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_HqdataRdy) { + for (size_t pos = 0; pos < _countof(_cached_previous_Hqdata.node_hq); ++pos) { + nodebuffer[nodeCount++] = node_hq.node_hq[pos]; + } + } + _cached_previous_Hqdata = node_hq; + _is_previous_HqdataRdy = true; + + } + + sl_result _cacheHqScanData() + { + sl_lidar_response_hq_capsule_measurement_nodes_t hq_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + _waitHqNode(hq_node); + while (_isScanning) { + ans = _waitHqNode(hq_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + + _HqToNormal(hq_node, local_buf, count); + for (size_t pos = 0; pos < count; ++pos){ + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT){ + // only publish the data when it contains a full 360 degree scan + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + + } + return SL_RESULT_OK; + } + + sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + return SL_RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) { + return SL_RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit 1 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + case 1: // expect the sync bit 2 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)) { + // calc the checksum ... + sl_u8 checksum = 0; + sl_u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); + + for (size_t cpos = offsetof(sl_lidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + + if (recvChecksum == checksum) { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) { + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + return SL_RESULT_OK; + } + return SL_RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _cacheUltraCapsuledScanData() + { + sl_lidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitUltraCapsuledNode(ultra_capsule_node); + + while (_isScanning) { + ans = _waitUltraCapsuledNode(ultra_capsule_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + + _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + + _isScanning = false; + + return SL_RESULT_OK; + } + sl_result _clearRxDataCache() + { + if (!isConnected()) + return SL_RESULT_OPERATION_FAIL; + _channel->flush(); + return SL_RESULT_OK; + } + + private: + IChannel *_channel; + bool _isConnected; + bool _isScanning; + MotorCtrlSupport _isSupportingMotorCtrl; + rp::hal::Locker _lock; + rp::hal::Event _dataEvt; + rp::hal::Thread _cachethread; + sl_u16 _cached_sampleduration_std; + sl_u16 _cached_sampleduration_express; + bool _scan_node_synced; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; + size_t _cached_scan_node_hq_count; + sl_u8 _cached_capsule_flag; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_hq_count_for_interval_retrieve; + + sl_lidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; + bool _is_previous_capsuledataRdy; + bool _is_previous_HqdataRdy; + }; + + Result createLidarDriver() + { + return new SlamtecLidarDriver(); + } +} \ No newline at end of file diff --git a/sdk/src/sl_serial_channel.cpp b/sdk/src/sl_serial_channel.cpp new file mode 100644 index 00000000..516cd4e2 --- /dev/null +++ b/sdk/src/sl_serial_channel.cpp @@ -0,0 +1,121 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class SerialPortChannel : public ISerialPortChannel + { + public: + SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()) + { + _device = device; + _baudrate = baudrate; + } + + ~SerialPortChannel() + { + if (_rxtxSerial) + delete _rxtxSerial; + } + + bool bind(const std::string& device, sl_s32 baudrate) + { + _closePending = false; + return _rxtxSerial->bind(device.c_str(), baudrate); + } + + bool open() + { + if(!bind(_device, _baudrate)) + return false; + return _rxtxSerial->open(); + } + + void close() + { + _closePending = true; + _rxtxSerial->cancelOperation(); + _rxtxSerial->close(); + } + void flush() + { + _rxtxSerial->flush(0); + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (_closePending) return false; + return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK); + } + + int write(const void* data, size_t size) + { + return _rxtxSerial->senddata((const sl_u8 * )data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size); + return lenRec; + } + + void clearReadCache() + { + + } + + void setDTR(bool dtr) + { + dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR(); + } + + private: + rp::hal::serial_rxtx * _rxtxSerial; + bool _closePending; + std::string _device; + int _baudrate; + + }; + + Result createSerialPortChannel(const std::string& device, int baudrate) + { + return new SerialPortChannel(device, baudrate); + } + +} \ No newline at end of file diff --git a/sdk/src/sl_tcp_channel.cpp b/sdk/src/sl_tcp_channel.cpp new file mode 100644 index 00000000..f4fd5ae3 --- /dev/null +++ b/sdk/src/sl_tcp_channel.cpp @@ -0,0 +1,106 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class TcpChannel : public IChannel + { + public: + TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return SL_RESULT_OK; + } + + bool open() + { + if(SL_IS_FAIL(bind(_ip, _port))) + return false; + return IS_OK(_binded_socket->connect(_socket)); + + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->send(data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + _binded_socket->recv(buffer, size, lenRec); + return lenRec; + } + + void clearReadCache() {} + + void setStatus(_u32 flag){} + private: + rp::net::StreamSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + Result createTcpChannel(const std::string& ip, int port) + { + return new TcpChannel(ip, port); + } +} \ No newline at end of file diff --git a/sdk/src/sl_udp_channel.cpp b/sdk/src/sl_udp_channel.cpp new file mode 100644 index 00000000..5bbca821 --- /dev/null +++ b/sdk/src/sl_udp_channel.cpp @@ -0,0 +1,116 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + class UdpChannel : public IChannel + { + public: + UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return SL_RESULT_OK; + } + + bool open() + { + if(SL_IS_FAIL(bind(_ip, _port))) + return false; + return SL_IS_OK(_binded_socket->setPairAddress(&_socket)); + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->sendTo(NULL, data, size); + } + + int read(void* buffer, size_t size) + { + u_result ans; + size_t recCnt = 0; + size_t lenRec = 0; + while (size > recCnt) + { + sl_u8 *temp = (sl_u8 *)buffer+recCnt; + ans = _binded_socket->recvFrom(temp, size, lenRec); + recCnt += lenRec; + if (ans) + break; + } + return recCnt; + + } + + void clearReadCache() {} + + void setStatus(_u32 flag){} + + private: + rp::net::DGramSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + + Result createUdpChannel(const std::string& ip, int port) + { + return new UdpChannel(ip, port); + } +} \ No newline at end of file diff --git a/src/node.cpp b/src/node.cpp index 59837ba2..5a3375c2 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -35,7 +35,7 @@ #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" #include "std_srvs/Empty.h" -#include "rplidar.h" +#include "sl_lidar.h" #ifndef _countof #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) @@ -43,12 +43,12 @@ #define DEG2RAD(x) ((x)*M_PI/180.) -using namespace rp::standalone::rplidar; +using namespace sl; -RPlidarDriver * drv = NULL; +ILidarDriver * drv = NULL; void publish_scan(ros::Publisher *pub, - rplidar_response_measurement_node_hq_t *nodes, + sl_lidar_response_measurement_node_hq_t *nodes, size_t node_count, ros::Time start, double scan_time, bool inverted, float angle_min, float angle_max, @@ -61,7 +61,7 @@ void publish_scan(ros::Publisher *pub, scan_msg.header.stamp = start; scan_msg.header.frame_id = frame_id; scan_count++; - + bool reversed = (angle_max > angle_min); if ( reversed ) { scan_msg.angle_min = M_PI - angle_max; @@ -104,14 +104,14 @@ void publish_scan(ros::Publisher *pub, pub->publish(scan_msg); } -bool getRPLIDARDeviceInfo(RPlidarDriver * drv) +bool getRPLIDARDeviceInfo(ILidarDriver * drv) { - u_result op_result; - rplidar_response_device_info_t devinfo; + sl_result op_result; + sl_lidar_response_device_info_t devinfo; op_result = drv->getDeviceInfo(devinfo); - if (IS_FAIL(op_result)) { - if (op_result == RESULT_OPERATION_TIMEOUT) { + if (SL_IS_FAIL(op_result)) { + if (op_result == SL_RESULT_OPERATION_TIMEOUT) { ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! "); } else { ROS_ERROR("Error, unexpected error, code: %x",op_result); @@ -120,30 +120,35 @@ bool getRPLIDARDeviceInfo(RPlidarDriver * drv) } // print out the device serial number, firmware and hardware version number.. - printf("RPLIDAR S/N: "); + char sn_str[35] = {0}; for (int pos = 0; pos < 16 ;++pos) { - printf("%02X", devinfo.serialnum[pos]); + sprintf(sn_str + (pos * 2),"%02X", devinfo.serialnum[pos]); } - printf("\n"); + ROS_INFO("RPLIDAR S/N: %s",sn_str); ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF); ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version); return true; } -bool checkRPLIDARHealth(RPlidarDriver * drv) +bool checkRPLIDARHealth(ILidarDriver * drv) { - u_result op_result; - rplidar_response_device_health_t healthinfo; + sl_result op_result; + sl_lidar_response_device_health_t healthinfo; + op_result = drv->getHealth(healthinfo); - if (IS_OK(op_result)) { - ROS_INFO("RPLidar health status : %d", healthinfo.status); - if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry."); - return false; - } else { - return true; + if (SL_IS_OK(op_result)) { + //ROS_INFO("RPLidar health status : %d", healthinfo.status); + switch (healthinfo.status) { + case SL_LIDAR_STATUS_OK: + ROS_INFO("RPLidar health status : OK."); + return true; + case SL_LIDAR_STATUS_WARNING: + ROS_INFO("RPLidar health status : Warning."); + return true; + case SL_LIDAR_STATUS_ERROR: + ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry."); + return false; } - } else { ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result); return false; @@ -157,7 +162,7 @@ bool stop_motor(std_srvs::Empty::Request &req, return false; ROS_DEBUG("Stop motor"); - drv->stopMotor(); + drv->setMotorSpeed(0); return true; } @@ -169,7 +174,7 @@ bool start_motor(std_srvs::Empty::Request &req, if(drv->isConnected()) { ROS_DEBUG("Start motor"); - u_result ans=drv->startMotor(); + sl_result ans=drv->setMotorSpeed(); ans=drv->startScan(0,1); } @@ -177,7 +182,7 @@ bool start_motor(std_srvs::Empty::Request &req, return true; } -static float getAngle(const rplidar_response_measurement_node_hq_t& node) +static float getAngle(const sl_lidar_response_measurement_node_hq_t& node) { return node.angle_z_q14 * 90.f / 16384.f; } @@ -187,119 +192,127 @@ int main(int argc, char * argv[]) { std::string channel_type; std::string tcp_ip; - std::string serial_port; int tcp_port = 20108; + std::string udp_ip; + int udp_port = 8089; + std::string serial_port; int serial_baudrate = 115200; std::string frame_id; bool inverted = false; - bool angle_compensate = true; - float max_distance = 8.0; - int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree + bool angle_compensate = true; + float angle_compensate_multiple = 1.0;//min 360 ponits at per 1 degree + int points_per_circle = 360;//min 360 ponits at per circle std::string scan_mode; + float max_distance; + float scan_frequency; ros::NodeHandle nh; ros::Publisher scan_pub = nh.advertise("scan", 1000); ros::NodeHandle nh_private("~"); nh_private.param("channel_type", channel_type, "serial"); nh_private.param("tcp_ip", tcp_ip, "192.168.0.7"); nh_private.param("tcp_port", tcp_port, 20108); + nh_private.param("udp_ip", udp_ip, "192.168.11.2"); + nh_private.param("udp_port", udp_port, 8089); nh_private.param("serial_port", serial_port, "/dev/ttyUSB0"); nh_private.param("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3 nh_private.param("frame_id", frame_id, "laser_frame"); nh_private.param("inverted", inverted, false); nh_private.param("angle_compensate", angle_compensate, false); nh_private.param("scan_mode", scan_mode, std::string()); + if(channel_type == "udp"){ + nh_private.param("scan_frequency", scan_frequency, 20.0); + } + else{ + nh_private.param("scan_frequency", scan_frequency, 10.0); + } - ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION""); + int ver_major = SL_LIDAR_SDK_VERSION_MAJOR; + int ver_minor = SL_LIDAR_SDK_VERSION_MINOR; + int ver_patch = SL_LIDAR_SDK_VERSION_PATCH; + ROS_INFO("RPLIDAR running on ROS package rplidar_ros, SDK Version:%d.%d.%d",ver_major,ver_minor,ver_patch); - u_result op_result; + sl_result op_result; // create the driver instance + drv = *createLidarDriver(); + IChannel* _channel; if(channel_type == "tcp"){ - drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP); + _channel = *createTcpChannel(tcp_ip, tcp_port); } - else{ - drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); + else if(channel_type == "udp"){ + _channel = *createUdpChannel(udp_ip, udp_port); } - - - if (!drv) { - ROS_ERROR("Create Driver fail, exit"); - return -2; + else{ + _channel = *createSerialPortChannel(serial_port, serial_baudrate); } - - if(channel_type == "tcp"){ - // make connection... - if (IS_FAIL(drv->connect(tcp_ip.c_str(), (_u32)tcp_port))) { - ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); - RPlidarDriver::DisposeDriver(drv); - return -1; + if (SL_IS_FAIL((drv)->connect(_channel))) { + if(channel_type == "tcp"){ + ROS_ERROR("Error, cannot connect to the ip addr %s with the tcp port %s.",tcp_ip.c_str(),std::to_string(tcp_port).c_str()); } - - } - else{ - // make connection... - if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) { - ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); - RPlidarDriver::DisposeDriver(drv); - return -1; + else if(channel_type == "udp"){ + ROS_ERROR("Error, cannot connect to the ip addr %s with the udp port %s.",udp_ip.c_str(),std::to_string(udp_port).c_str()); } - + else{ + ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); + } + delete drv; + return -1; } - // get rplidar device info - if (!getRPLIDARDeviceInfo(drv)) { - return -1; + if(!getRPLIDARDeviceInfo(drv)){ + delete drv; + return -1; } - - // check health... if (!checkRPLIDARHealth(drv)) { - RPlidarDriver::DisposeDriver(drv); + delete drv; return -1; } - + + //two service for start/stop lidar rotate ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor); ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); - drv->startMotor(); + //start lidar rotate + drv->setMotorSpeed(); - RplidarScanMode current_scan_mode; + LidarScanMode current_scan_mode; if (scan_mode.empty()) { op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode); } else { - std::vector allSupportedScanModes; + std::vector allSupportedScanModes; op_result = drv->getAllSupportedScanModes(allSupportedScanModes); - if (IS_OK(op_result)) { - _u16 selectedScanMode = _u16(-1); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { + if (SL_IS_OK(op_result)) { + sl_u16 selectedScanMode = sl_u16(-1); + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { if (iter->scan_mode == scan_mode) { selectedScanMode = iter->id; break; } } - if (selectedScanMode == _u16(-1)) { + if (selectedScanMode == sl_u16(-1)) { ROS_ERROR("scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str()); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { ROS_ERROR("\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode, iter->max_distance, (1000/iter->us_per_sample)); } - op_result = RESULT_OPERATION_FAIL; + op_result = SL_RESULT_OPERATION_FAIL; } else { op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode); } } } - if(IS_OK(op_result)) + if(SL_IS_OK(op_result)) { - //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us - angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0); + //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us + points_per_circle = (int)(1000*1000/current_scan_mode.us_per_sample/scan_frequency); + angle_compensate_multiple = points_per_circle/360.0 + 1; if(angle_compensate_multiple < 1) - angle_compensate_multiple = 1; - max_distance = current_scan_mode.max_distance; - ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode, - current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple); + angle_compensate_multiple = 1.0; + max_distance = (float)current_scan_mode.max_distance; + ROS_INFO("current scan mode: %s, sample rate: %d Khz, max_distance: %.1f m, scan frequency:%.1f Hz, ", current_scan_mode.scan_mode,(int)(1000/current_scan_mode.us_per_sample+0.5),max_distance, scan_frequency); } else { @@ -310,7 +323,7 @@ int main(int argc, char * argv[]) { ros::Time end_scan_time; double scan_duration; while (ros::ok()) { - rplidar_response_measurement_node_hq_t nodes[360*8]; + sl_lidar_response_measurement_node_hq_t nodes[8192]; size_t count = _countof(nodes); start_scan_time = ros::Time::now(); @@ -318,17 +331,16 @@ int main(int argc, char * argv[]) { end_scan_time = ros::Time::now(); scan_duration = (end_scan_time - start_scan_time).toSec(); - if (op_result == RESULT_OK) { + if (op_result == SL_RESULT_OK) { op_result = drv->ascendScanData(nodes, count); float angle_min = DEG2RAD(0.0f); - float angle_max = DEG2RAD(359.0f); - if (op_result == RESULT_OK) { + float angle_max = DEG2RAD(360.0f); + if (op_result == SL_RESULT_OK) { if (angle_compensate) { - //const int angle_compensate_multiple = 1; - const int angle_compensate_nodes_count = 360*angle_compensate_multiple; + const int angle_compensate_nodes_count = 360*angle_compensate_multiple; int angle_compensate_offset = 0; - rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count]; - memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t)); + sl_lidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count]; + memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(sl_lidar_response_measurement_node_hq_t)); int i = 0, j = 0; for( ; i < count; i++ ) { @@ -368,7 +380,7 @@ int main(int argc, char * argv[]) { angle_min, angle_max, max_distance, frame_id); } - } else if (op_result == RESULT_OPERATION_FAIL) { + } else if (op_result == SL_RESULT_OPERATION_FAIL) { // All the data is invalid, just publish them float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); @@ -383,8 +395,8 @@ int main(int argc, char * argv[]) { } // done! - drv->stopMotor(); + drv->setMotorSpeed(0); drv->stop(); - RPlidarDriver::DisposeDriver(drv); + delete drv; return 0; }