From dcd2dd87ce8034a46e4150a89dc69f6ba1a22d8d Mon Sep 17 00:00:00 2001 From: Edvin Date: Thu, 10 Apr 2025 11:35:25 +0200 Subject: Implemented code for Slamtec rplidar C1 --- LIDAR/sdk/include/rplidar.h | 45 +++ LIDAR/sdk/include/rplidar_cmd.h | 215 ++++++++++++ LIDAR/sdk/include/rplidar_driver.h | 247 ++++++++++++++ LIDAR/sdk/include/rplidar_protocol.h | 61 ++++ LIDAR/sdk/include/rptypes.h | 116 +++++++ LIDAR/sdk/include/sl_crc.h | 43 +++ LIDAR/sdk/include/sl_lidar.h | 47 +++ LIDAR/sdk/include/sl_lidar_cmd.h | 388 +++++++++++++++++++++ LIDAR/sdk/include/sl_lidar_driver.h | 569 +++++++++++++++++++++++++++++++ LIDAR/sdk/include/sl_lidar_driver_impl.h | 157 +++++++++ LIDAR/sdk/include/sl_lidar_protocol.h | 85 +++++ LIDAR/sdk/include/sl_types.h | 83 +++++ 12 files changed, 2056 insertions(+) create mode 100644 LIDAR/sdk/include/rplidar.h create mode 100644 LIDAR/sdk/include/rplidar_cmd.h create mode 100644 LIDAR/sdk/include/rplidar_driver.h create mode 100644 LIDAR/sdk/include/rplidar_protocol.h create mode 100644 LIDAR/sdk/include/rptypes.h create mode 100644 LIDAR/sdk/include/sl_crc.h create mode 100644 LIDAR/sdk/include/sl_lidar.h create mode 100644 LIDAR/sdk/include/sl_lidar_cmd.h create mode 100644 LIDAR/sdk/include/sl_lidar_driver.h create mode 100644 LIDAR/sdk/include/sl_lidar_driver_impl.h create mode 100644 LIDAR/sdk/include/sl_lidar_protocol.h create mode 100644 LIDAR/sdk/include/sl_types.h (limited to 'LIDAR/sdk/include') diff --git a/LIDAR/sdk/include/rplidar.h b/LIDAR/sdk/include/rplidar.h new file mode 100644 index 0000000..c4f4752 --- /dev/null +++ b/LIDAR/sdk/include/rplidar.h @@ -0,0 +1,45 @@ +/* + * 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. + * + */ + +#pragma once + +#include +#include "hal/types.h" +#include "rplidar_protocol.h" +#include "rplidar_cmd.h" + +#include "rplidar_driver.h" + +#define RPLIDAR_SDK_VERSION "2.0.0" +#define SLAMTEC_LIDAR_SDK_VERSION SL_LIDAR_SDK_VERSION diff --git a/LIDAR/sdk/include/rplidar_cmd.h b/LIDAR/sdk/include/rplidar_cmd.h new file mode 100644 index 0000000..6bfc7e7 --- /dev/null +++ b/LIDAR/sdk/include/rplidar_cmd.h @@ -0,0 +1,215 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * 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_cmd.h" +#include "rplidar_protocol.h" + +// Commands +//----------------------------------------- + +#define RPLIDAR_AUTOBAUD_MAGICBYTE SL_LIDAR_AUTOBAUD_MAGICBYTE + +// Commands without payload and response +#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 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_HQ_MOTOR_SPEED_CTRL SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL + +// 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 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 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) +#endif + + +// Payloads +// ------------------------------------------ +#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 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 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 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 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 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 SL_LIDAR_ANS_TYPE_SAMPLE_RATE +//added in FW ver 1.23alpha +#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 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_MEASUREMENT_ULTRA_DENSE_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_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 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 SL_LIDAR_STATUS_OK +#define RPLIDAR_STATUS_WARNING SL_LIDAR_STATUS_WARNING +#define RPLIDAR_STATUS_ERROR SL_LIDAR_STATUS_ERROR + +#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 + +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 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; + + +#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 + + +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; +typedef sl_lidar_response_ultra_dense_capsule_measurement_nodes_t rplidar_response_ultra_dense_capsule_measurement_nodes_t; +// ext1 : x2 boost mode + +#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 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; + + +# 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_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 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 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() +#endif diff --git a/LIDAR/sdk/include/rplidar_driver.h b/LIDAR/sdk/include/rplidar_driver.h new file mode 100644 index 0000000..bd2fc82 --- /dev/null +++ b/LIDAR/sdk/include/rplidar_driver.h @@ -0,0 +1,247 @@ +/* + * 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. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +#ifndef __cplusplus +#error "The RPlidar SDK requires a C++ compiler to be built" +#endif + + +namespace rp { namespace standalone{ namespace rplidar { + using namespace sl; + typedef LidarScanMode RplidarScanMode; + +enum { + DRIVER_TYPE_SERIALPORT = 0x0, + DRIVER_TYPE_TCP = 0x1, + DRIVER_TYPE_UDP = 0x2, +}; + +class RPlidarDriver { +public: + enum { + DEFAULT_TIMEOUT = 2000, //2000 ms + }; + + enum { + MAX_SCAN_NODES = 8192, + }; + + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + +public: + /// Create an RPLIDAR Driver Instance + /// This interface should be invoked first before any other operations + /// + /// \param drivertype the connection type used by the driver. + 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 + /// e.g. on Windows, it may be com3 or \\.\com10 + /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + /// + /// \param baudrate the baudrate used + /// For most RPLIDAR models, the baudrate should be set to 115200 + /// + /// \param flag other flags + /// Reserved for future use, always set to Zero + u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0); + + /// Disconnect with the RPLIDAR and close the serial port + void disconnect(); + + /// Returns TRUE when the connection has been established + 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 + u_result reset(_u32 timeout = DEFAULT_TIMEOUT); + + u_result clearNetSerialRxCache() { + return RESULT_OK; + } + // FW1.24 + /// Get all scan modes that supported by lidar + u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Get typical scan mode of lidar + u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// 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 + u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); + + /// 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 + 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. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + 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 + u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); + + /// 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 + u_result setMotorPWM(_u16 pwm); + + /// Start RPLIDAR's motor when using accessory board + u_result startMotor(); + + /// Stop RPLIDAR's motor when using accessory board + 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. + u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); + + ///Set LPX and S2E 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); + + ///Get LPX and S2E 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 getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); + + ///Get LPX and S2E 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 + 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: + /// + /// 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. + 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. + /// + /// \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. + u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); + + /// 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 RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); + + /// 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 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(); +protected: + RPlidarDriver(); + +private: + sl_u32 _channelType; + IChannel* _channel; + ILidarDriver* _lidarDrv; + +}; + + + + +}}} diff --git a/LIDAR/sdk/include/rplidar_protocol.h b/LIDAR/sdk/include/rplidar_protocol.h new file mode 100644 index 0000000..cddb9c8 --- /dev/null +++ b/LIDAR/sdk/include/rplidar_protocol.h @@ -0,0 +1,61 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * 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_protocol.h" +// RP-Lidar Input Packets + +#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 SL_LIDAR_ANS_SYNC_BYTE1 +#define RPLIDAR_ANS_SYNC_BYTE2 SL_LIDAR_ANS_SYNC_BYTE2 + +#define RPLIDAR_ANS_PKTFLAG_LOOP SL_LIDAR_ANS_PKTFLAG_LOOP + +#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 sl_lidar_cmd_packet_t rplidar_cmd_packet_t; +typedef sl_lidar_ans_header_t rplidar_ans_header_t; + + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/LIDAR/sdk/include/rptypes.h b/LIDAR/sdk/include/rptypes.h new file mode 100644 index 0000000..34d8ecb --- /dev/null +++ b/LIDAR/sdk/include/rptypes.h @@ -0,0 +1,116 @@ +/* + * 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. + * + */ + +#pragma once + + +#ifdef _WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + +#else + +#include + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#define __small_endian + +#ifndef __GNUC__ +#define __attribute__(x) +#endif + + +// The _word_size_t uses actual data bus width of the current CPU +#ifdef _AVR_ +typedef _u8 _word_size_t; +#define THREAD_PROC +#elif defined (WIN64) +typedef _u64 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (WIN32) +typedef _u32 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (__GNUC__) +typedef unsigned long _word_size_t; +#define THREAD_PROC +#elif defined (__ICCARM__) +typedef _u32 _word_size_t; +#define THREAD_PROC +#endif + + +typedef uint32_t u_result; + +#define RESULT_OK 0 +#define RESULT_FAIL_BIT 0x80000000 +#define RESULT_ALREADY_DONE 0x20 +#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) +#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) +#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) + +#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) +#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) + +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); diff --git a/LIDAR/sdk/include/sl_crc.h b/LIDAR/sdk/include/sl_crc.h new file mode 100644 index 0000000..184a8df --- /dev/null +++ b/LIDAR/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/LIDAR/sdk/include/sl_lidar.h b/LIDAR/sdk/include/sl_lidar.h new file mode 100644 index 0000000..db15fe7 --- /dev/null +++ b/LIDAR/sdk/include/sl_lidar.h @@ -0,0 +1,47 @@ +/* +* 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 1 +#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_MK_STR_INDIR(x) #x +#define SL_LIDAR_SDK_VERSION_MK_STR(x) SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x) + +#define SL_LIDAR_SDK_VERSION (SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MAJOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MINOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_PATCH)) diff --git a/LIDAR/sdk/include/sl_lidar_cmd.h b/LIDAR/sdk/include/sl_lidar_cmd.h new file mode 100644 index 0000000..bd3f8d2 --- /dev/null +++ b/LIDAR/sdk/include/sl_lidar_cmd.h @@ -0,0 +1,388 @@ +/* +* 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 + +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable:4200) +#endif + +#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; +} __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.23alpha +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED 0x86 + + +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE 0x15 + +//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_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; + + +typedef struct _sl_lidar_response_ultra_dense_cabin_nodes_t { + sl_u16 qualityl_distance_scale[2]; + sl_u8 qualityh_array; +} __attribute__((packed)) sl_lidar_response_ultra_dense_cabin_nodes_t; + +typedef struct _sl_lidar_response_ultra_dense_capsule_measurement_nodes_t { + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u32 time_stamp; + sl_u16 dev_status; + sl_u16 start_angle_sync_q6; + sl_lidar_response_ultra_dense_cabin_nodes_t cabins[32]; +} __attribute__((packed)) sl_lidar_response_ultra_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_MODEL_REVISION_ID 0x00000080 +#define SL_LIDAR_CONF_MODEL_NAME_ALIAS 0x00000081 + +#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 type; + 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 + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif \ No newline at end of file diff --git a/LIDAR/sdk/include/sl_lidar_driver.h b/LIDAR/sdk/include/sl_lidar_driver.h new file mode 100644 index 0000000..9dade3c --- /dev/null +++ b/LIDAR/sdk/include/sl_lidar_driver.h @@ -0,0 +1,569 @@ +/* + * 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; + } + }; + + enum LIDARTechnologyType { + LIDAR_TECHNOLOGY_UNKNOWN = 0, + LIDAR_TECHNOLOGY_TRIANGULATION = 1, + LIDAR_TECHNOLOGY_DTOF = 2, + LIDAR_TECHNOLOGY_ETOF = 3, + LIDAR_TECHNOLOGY_FMCW = 4, + }; + + enum LIDARMajorType { + LIDAR_MAJOR_TYPE_UNKNOWN = 0, + LIDAR_MAJOR_TYPE_A_SERIES = 1, + LIDAR_MAJOR_TYPE_S_SERIES = 2, + LIDAR_MAJOR_TYPE_T_SERIES = 3, + LIDAR_MAJOR_TYPE_M_SERIES = 4, + LIDAR_MAJOR_TYPE_C_SERIES = 6, + }; + + enum LIDARInterfaceType { + LIDAR_INTERFACE_UART = 0, + LIDAR_INTERFACE_ETHERNET = 1, + LIDAR_INTERFACE_USB = 2, + LIDAR_INTERFACE_CANBUS = 5, + + + LIDAR_INTERFACE_UNKNOWN = 0xFFFF, + }; + + struct SlamtecLidarTimingDesc { + + sl_u32 sample_duration_uS; + sl_u32 native_baudrate; + + sl_u32 linkage_delay_uS; + + LIDARInterfaceType native_interface_type; + + bool native_timestamp_support; + }; + + /** + * 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; + + + /** + * Wait for some data + * \param size_hint Byte count may available to retrieve without beening blocked + * \param timeoutInMs Wait timeout (in microseconds, -1 for forever) + * \return RESULT_OK if there is data available for receiving + * RESULT_OPERATION_TIMEOUT if the given timeout duration is exceed + * RESULT_OPERATION_FAIL if there is something wrong with the channel + */ + virtual sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs = 1000) = 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; + + virtual int getChannelType() = 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 and S2E 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 and S2E 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 getLidarIpConf( 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 + /// Notice: the macAddrArray must point to a valid buffer with at least 6 bytes length + /// Otherwise, buffer overwrite will occur + 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; + + + + /// Wait and grab a complete 0-360 degree scan data previously received with timestamp support. + /// + /// The returned timestamp belongs to the first data point of the scan data (begining of the scan). + /// Its value is represented based on the current machine's time domain with the unit of microseconds (uS). + /// + /// If the currently connected LIDAR supports hardware timestamp mechanism, this timestamp will use + /// the actual data emitted by the LIDAR device and remap it to the current machine's time domain. + /// + /// For other models that do not support hardware timestamps, this data will be deducted through estimation, + /// and there may be a slight deviation from the actual situation. + /// + /// 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 timestamp_uS The reference used to store the timestamp value. + /// \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 grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64 & timestamp_uS, 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; + + + + /// Get the technology of the LIDAR's measurement system + /// + /// + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + virtual LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0; + + + /// Get the Major Type (Series Info) of the LIDAR + /// + /// + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + virtual LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0; + + + /// Get the Model Name of the LIDAR + /// The result will be somthing like: "A1M8" or "S1M1" or "A3M1-R1" + /// + /// \param out_description The output string that contains the generated model name + /// + /// \param fetchAliasName If set to true, a communication will be taken to ask if there is any Alias name availabe + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + /// \param timeout The timeout value used by potential data communication + virtual sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName = true, const sl_lidar_response_device_info_t* devInfo = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 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/LIDAR/sdk/include/sl_lidar_driver_impl.h b/LIDAR/sdk/include/sl_lidar_driver_impl.h new file mode 100644 index 0000000..3c1b814 --- /dev/null +++ b/LIDAR/sdk/include/sl_lidar_driver_impl.h @@ -0,0 +1,157 @@ +/* + * 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 getLidarIpConf(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, sl_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(); + + void _ultra_dense_capsuleToNormal(const sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& capslue, sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& nodeCount); + sl_result _waitUltraDenseCapsuledNode(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheUltraDenseCapsuledScanData(); + + + 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/LIDAR/sdk/include/sl_lidar_protocol.h b/LIDAR/sdk/include/sl_lidar_protocol.h new file mode 100644 index 0000000..a9a10c1 --- /dev/null +++ b/LIDAR/sdk/include/sl_lidar_protocol.h @@ -0,0 +1,85 @@ +/* +* 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 + + +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable:4200) +#endif + +#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 + + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif \ No newline at end of file diff --git a/LIDAR/sdk/include/sl_types.h b/LIDAR/sdk/include/sl_types.h new file mode 100644 index 0000000..96a3755 --- /dev/null +++ b/LIDAR/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) ) -- cgit v1.2.3