Implemented code for Slamtec rplidar C1

12 files changed, 2056 insertions, 0 deletions

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 <vector>
+#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<RplidarScanMode>& 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 <stdint.h>
+
+#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 <vector>
+#include <map>
+#include <string>
+
+#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 <string>
+
+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 <typename T>
+    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<IChannel*> createSerialPortChannel(const std::string& device, int baudrate);
+
+    /**
+    * Create a TCP channel
+    * \param ip IP address of the device
+    * \param port TCP port
+    */
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port);
+
+    /**
+    * Create a UDP channel
+    * \param ip IP address of the device
+    * \param port UDP port
+    */
+    Result<IChannel*> 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<LidarScanMode>& 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<ISerialChannel*> 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<ILidarDriver*> 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<LidarScanMode>& 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<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), 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 <typename T>
+			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 <cstdint>
+
+#define SL_DEFINE_TYPE(IntType, NewType)    typedef std::IntType NewType
+#else
+#include <stdint.h>
+
+#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) )