Implemented code for Slamtec rplidar C1

55 files changed, 11906 insertions, 0 deletions

diff --git a/LIDAR/sdk/src/arch/linux/arch_linux.h b/LIDAR/sdk/src/arch/linux/arch_linux.h
new file mode 100644
index 0000000..ce31343
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/arch_linux.h
@@ -0,0 +1,64 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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
+
+// libc dep
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <time.h>
+#include <stdarg.h>
+
+// libc++ dep
+#include <iostream>
+#include <string>
+
+// linux specific
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <time.h>
+
+#include "timer.h"
+
diff --git a/LIDAR/sdk/src/arch/linux/net_serial.cpp b/LIDAR/sdk/src/arch/linux/net_serial.cpp
new file mode 100644
index 0000000..2bec556
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_serial.cpp
@@ -0,0 +1,475 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+// linux specific
+
+#include <errno.h>
+#include <fcntl.h>
+
+#include <time.h>
+#include "hal/types.h"
+#include "arch/linux/net_serial.h"
+#include <sys/select.h>
+
+#include <algorithm>
+//__GNUC__
+#if defined(__GNUC__)
+// for Linux extension
+#include <asm/ioctls.h>
+#include <asm/termbits.h>
+#include <sys/ioctl.h>
+extern "C" int tcflush(int fildes, int queue_selector);
+#else
+// for other standard UNIX
+#include <termios.h>
+#include <sys/ioctl.h>
+
+#endif
+
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _baudrate(0)
+    , _flags(0)
+    , serial_fd(-1)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
+{
+    if (isOpened()) close();
+    
+    serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+
+    if (serial_fd == -1) return false;
+
+    
+
+#if !defined(__GNUC__)
+    // for standard UNIX
+    struct termios options, oldopt;
+    tcgetattr(serial_fd, &oldopt);
+    bzero(&options,sizeof(struct termios));
+
+    // enable rx and tx
+    options.c_cflag |= (CLOCAL | CREAD);
+
+    _u32 termbaud = getTermBaudBitmap(baudrate);
+
+    if (termbaud == (_u32)-1) {
+        close();
+        return false;
+    }
+    cfsetispeed(&options, termbaud);
+    cfsetospeed(&options, termbaud);
+
+    options.c_cflag &= ~PARENB; //no checkbit
+    options.c_cflag &= ~CSTOPB; //1bit stop bit
+    options.c_cflag &= ~CRTSCTS; //no flow control
+
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8; /* Select 8 data bits */
+
+#ifdef CNEW_RTSCTS
+    options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+    // raw input mode   
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    options.c_oflag &= ~OPOST;
+
+
+
+    if (tcsetattr(serial_fd, TCSANOW, &options))
+    {
+        close();
+        return false;
+    }
+
+#else
+
+    // using Linux extension ...
+    struct termios2 tio;
+
+    ioctl(serial_fd, TCGETS2, &tio);
+    bzero(&tio, sizeof(struct termios2));
+
+    tio.c_cflag = BOTHER;
+    tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
+
+    tio.c_cflag &= ~CSTOPB;   //1 stop bit
+    tio.c_cflag &= ~CRTSCTS;  //No CTS
+    tio.c_cflag &= ~PARENB;   //No Parity
+
+#ifdef CNEW_RTSCTS
+    tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+
+    tio.c_cc[VMIN] = 0;         //min chars to read
+    tio.c_cc[VTIME] = 0;        //time in 1/10th sec wait
+
+    tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    tio.c_oflag &= ~OPOST;
+
+    tio.c_ispeed = baudrate;
+    tio.c_ospeed = baudrate;
+
+
+    ioctl(serial_fd, TCSETS2, &tio);
+
+#endif
+
+
+    tcflush(serial_fd, TCIFLUSH);
+
+    if (fcntl(serial_fd, F_SETFL, FNDELAY))
+    {
+        close();
+        return false;
+    }
+
+
+    _is_serial_opened = true;
+    _operation_aborted = false;
+
+    //Clear the DTR bit to let the motor spin
+    clearDTR();
+    do {
+        // create self pipeline for wait cancellation
+        if (pipe(_selfpipe) == -1) break;
+
+        int flags = fcntl(_selfpipe[0], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make read end nonblocking */
+        if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
+            break;
+
+        flags = fcntl(_selfpipe[1], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make write end nonblocking */
+        if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
+            break;
+
+    } while (0);
+    
+    return true;
+}
+
+void raw_serial::close()
+{
+    if (serial_fd != -1)
+        ::close(serial_fd);
+    serial_fd = -1;
+    
+    if (_selfpipe[0] != -1)
+        ::close(_selfpipe[0]);
+
+    if (_selfpipe[1] != -1)
+        ::close(_selfpipe[1]);
+
+    _selfpipe[0] = _selfpipe[1] = -1;
+
+    _operation_aborted = false;
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+// FIXME: non-block io should be used
+    if (!isOpened()) return 0;
+
+    if (data == NULL || size ==0) return 0;
+    
+    size_t tx_len = 0;
+    required_tx_cnt = 0;
+    do {
+        int ans = ::write(serial_fd, data + tx_len, size-tx_len);
+        
+        if (ans == -1) return tx_len;
+        
+        tx_len += ans;
+        required_tx_cnt = tx_len;
+    }while (tx_len<size);
+    
+    
+    return tx_len;
+}
+
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    
+    int ans = ::read(serial_fd, data, size);
+    
+    if (ans == -1) ans=0;
+    required_rx_cnt = ans;
+    return ans;
+}
+
+
+void raw_serial::flush( _u32 flags)
+{
+    tcflush(serial_fd,TCIFLUSH); 
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (returned_size) *returned_size = required_tx_cnt;
+    return 0;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+   
+    if (returned_size) *returned_size = required_rx_cnt;
+    return 0;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    size_t length = 0;
+    if (returned_size==NULL) returned_size=(size_t *)&length;
+    *returned_size = 0;
+
+    int max_fd;
+    fd_set input_set;
+    struct timeval timeout_val;
+
+    /* Initialize the input set */
+    FD_ZERO(&input_set);
+    FD_SET(serial_fd, &input_set);
+
+    if (_selfpipe[0] != -1)
+        FD_SET(_selfpipe[0], &input_set);
+
+    max_fd =  std::max<int>(serial_fd, _selfpipe[0]) + 1;
+
+    /* Initialize the timeout structure */
+    timeout_val.tv_sec = timeout / 1000;
+    timeout_val.tv_usec = (timeout % 1000) * 1000;
+
+    if ( isOpened() )
+    {
+        int nread;
+
+        if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR;
+
+        *returned_size = nread;
+
+        if (*returned_size >= data_count)
+        {
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        /* Do the select */
+        int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
+
+        if (n < 0)
+        {
+            // select error
+            *returned_size =  0;
+            return ANS_DEV_ERR;
+        }
+        else if (n == 0)
+        {
+            // time out
+            *returned_size =0;
+            return ANS_TIMEOUT;
+        }
+        else
+        {
+            if (FD_ISSET(_selfpipe[0], &input_set)) {   
+                // require aborting the current operation
+                int ch;
+                for (;;) {                    
+                    if (::read(_selfpipe[0], &ch, 1) == -1) {
+                        break;
+                    }
+                    
+                }
+
+                // treat as  timeout
+                *returned_size = 0;
+                return ANS_TIMEOUT;
+            }
+
+            // data avaliable
+            assert (FD_ISSET(serial_fd, &input_set));
+
+
+            if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
+            if (*returned_size >= data_count)
+            {
+                return 0;
+            }
+        }
+        
+    }
+
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    size_t remaining;
+    
+    if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
+    return remaining;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIS, &dtr_bit);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIC, &dtr_bit);
+}
+
+void raw_serial::_init()
+{
+    serial_fd = -1;  
+    _portName[0] = 0;
+    required_tx_cnt = required_rx_cnt = 0;
+    _operation_aborted = false;
+    _selfpipe[0] = _selfpipe[1] = -1;
+}
+
+void raw_serial::cancelOperation()
+{
+    _operation_aborted = true;
+    if (_selfpipe[1] == -1) return;
+
+    (int)::write(_selfpipe[1], "x", 1);
+}
+
+_u32 raw_serial::getTermBaudBitmap(_u32 baud)
+{
+#define BAUD_CONV( _baud_) case _baud_:  return B##_baud_ 
+switch (baud) {
+        BAUD_CONV(1200);
+        BAUD_CONV(1800);
+        BAUD_CONV(2400);
+        BAUD_CONV(4800);
+        BAUD_CONV(9600);
+        BAUD_CONV(19200);
+        BAUD_CONV(38400);
+        BAUD_CONV(57600);
+        BAUD_CONV(115200);
+        BAUD_CONV(230400);
+        BAUD_CONV(460800);
+        BAUD_CONV(500000);
+        BAUD_CONV(576000);
+        BAUD_CONV(921600);
+        BAUD_CONV(1000000);
+        BAUD_CONV(1152000);
+        BAUD_CONV(1500000);
+        BAUD_CONV(2000000);
+        BAUD_CONV(2500000);
+        BAUD_CONV(3000000);
+        BAUD_CONV(3500000);
+        BAUD_CONV(4000000);
+    }
+    return -1;
+}
+
+}}} //end rp::arch::net
+
+//begin rp::hal
+namespace rp{ namespace hal{
+
+serial_rxtx * serial_rxtx::CreateRxTx()
+{
+    return new rp::arch::net::raw_serial();
+}
+
+void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
+{
+    delete rxtx;
+}
+
+}} //end rp::hal
diff --git a/LIDAR/sdk/src/arch/linux/net_serial.h b/LIDAR/sdk/src/arch/linux/net_serial.h
new file mode 100644
index 0000000..627369a
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_serial.h
@@ -0,0 +1,90 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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 "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+    _u32 getTermBaudBitmap(_u32 baud);
+
+    virtual void cancelOperation();
+
+protected:
+    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    void _init();
+
+    char _portName[200];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    int serial_fd;
+
+    size_t required_tx_cnt;
+    size_t required_rx_cnt;
+
+    int    _selfpipe[2];
+    bool   _operation_aborted;
+};
+
+}}}
diff --git a/LIDAR/sdk/src/arch/linux/net_socket.cpp b/LIDAR/sdk/src/arch/linux/net_socket.cpp
new file mode 100644
index 0000000..99eb2dc
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_socket.cpp
@@ -0,0 +1,893 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ *
+ *  POXIS Implementation
+ */
+
+
+#include "sdkcommon.h"
+#include "../../hal/socket.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+
+
+namespace rp{ namespace net {
+
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{}
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, buffersize);
+        break;
+
+        case AF_INET6:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, buffersize);
+
+        break;
+    }
+    return ans==NULL?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
+
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+        int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (!ans) return RESULT_OK;
+
+
+        switch (errno) {
+            case EAFNOSUPPORT:
+                return RESULT_OPERATION_NOT_SUPPORT;
+#if 0
+            case EINPROGRESS:
+                return RESULT_OK; //treat async connection as good status
+#endif
+            case ETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        size_t addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , (socklen_t*)&addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL);
+        if (ans == len) {
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        }
+        
+    }
+
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, 0);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+            
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+#if 0
+    virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <=  sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SHUT_RD;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SHUT_WR;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SHUT_RDWR;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+    int  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+        const struct sockaddr * addr = target ? reinterpret_cast<const struct sockaddr *>(target->getPlatformData()) : NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != -1) {
+            assert(ans == len);
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+
+                case EMSGSIZE:
+                    return RESULT_INVALID_DATA;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        
+        }
+
+    }
+
+    virtual u_result setPairAddress(const SocketAddress* pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr);
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        return ans ? RESULT_OPERATION_FAIL : RESULT_OK;
+
+    }
+    
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+#if 0
+    virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
+
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+    
+protected:
+    int  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            return AF_PACKET;
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+
diff --git a/LIDAR/sdk/src/arch/linux/thread.hpp b/LIDAR/sdk/src/arch/linux/thread.hpp
new file mode 100644
index 0000000..6a9107b
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/thread.hpp
@@ -0,0 +1,185 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+#include <sched.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+
+    pid_t selfTid = syscall(SYS_gettid);
+
+        // check whether current schedule policy supports priority levels
+    int current_policy = SCHED_OTHER;
+    struct sched_param current_param;
+    int nice = 0;
+    int ans;
+
+    if (sched_getparam(selfTid, &current_param))
+    {
+        // cannot retreieve values
+        return RESULT_OPERATION_FAIL;
+    }   
+
+    int pthread_priority_min;
+
+#if 1
+    pthread_priority_min = sched_get_priority_min(SCHED_RR);
+#else
+    pthread_priority_min = 1;
+#endif
+	int pthread_priority = 0 ;
+
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		//pthread_priority = pthread_priority_max;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min + 1;
+        nice = 0;
+		break;
+	case PRIORITY_HIGH:
+		//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min;
+        nice = 0;
+		break;
+	case PRIORITY_NORMAL:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 0;
+        break;
+	case PRIORITY_LOW:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 10;
+        break;
+	case PRIORITY_IDLE:
+		pthread_priority = 0;
+        current_policy = SCHED_IDLE;
+        nice = 0;
+		break;
+	}
+    // change the inhertiable behavior
+    current_policy |= SCHED_RESET_ON_FORK;
+
+    current_param.__sched_priority = pthread_priority;
+
+  
+
+    
+    // do not use pthread version as it will make the priority be inherited by a thread child
+	if ( (ans = sched_setscheduler(selfTid, current_policy , &current_param)) )
+	{
+        if (ans == EPERM)
+        {
+            //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n");
+        }
+		return RESULT_OPERATION_FAIL;
+	}
+
+
+    if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH))
+    {
+        if (setpriority(PRIO_PROCESS, selfTid, nice)) {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+    
+
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+
+    int current_policy;
+    struct sched_param current_param;
+    if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
+    {
+        // cannot retreieve values
+        return PRIORITY_NORMAL;
+    }   
+
+    int pthread_priority_max = sched_get_priority_max(SCHED_RR);
+    int pthread_priority_min = sched_get_priority_min(SCHED_RR);
+
+    if (current_param.__sched_priority ==(pthread_priority_max ))
+	{
+		return PRIORITY_REALTIME;
+	}
+	if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
+	{
+		return PRIORITY_HIGH;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}
diff --git a/LIDAR/sdk/src/arch/linux/timer.cpp b/LIDAR/sdk/src/arch/linux/timer.cpp
new file mode 100644
index 0000000..a727e46
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/timer.cpp
@@ -0,0 +1,52 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+namespace rp{ namespace arch{
+_u64 rp_getus()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000000LL + t.tv_nsec/1000;
+}
+_u64 rp_getms()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000L + t.tv_nsec/1000000L;
+}
+}}
diff --git a/LIDAR/sdk/src/arch/linux/timer.h b/LIDAR/sdk/src/arch/linux/timer.h
new file mode 100644
index 0000000..1641001
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/timer.h
@@ -0,0 +1,59 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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 "hal/types.h"
+
+#include <unistd.h>
+static inline void delay(_word_size_t ms){
+    while (ms>=1000){
+        usleep(1000*1000);
+        ms-=1000;
+    };
+    if (ms!=0)
+        usleep(ms*1000);
+}
+
+// TODO: the highest timer interface should be clock_gettime
+namespace rp{ namespace arch{
+
+_u64 rp_getus();
+_u64 rp_getms();
+
+}}
+
+#define getms() rp::arch::rp_getms()
+#define getus() rp::arch::rp_getus()
+
diff --git a/LIDAR/sdk/src/arch/macOS/arch_macOS.h b/LIDAR/sdk/src/arch/macOS/arch_macOS.h
new file mode 100644
index 0000000..950ab97
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/arch_macOS.h
@@ -0,0 +1,66 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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
+
+// libc dep
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <time.h>
+#include <stdarg.h>
+
+// libc++ dep
+#include <iostream>
+#include <string>
+
+
+// POSIX specific
+extern "C" {
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <time.h>
+}
+
+#include "arch/macOS/timer.h"
+
diff --git a/LIDAR/sdk/src/arch/macOS/net_serial.cpp b/LIDAR/sdk/src/arch/macOS/net_serial.cpp
new file mode 100644
index 0000000..d1859fa
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/net_serial.cpp
@@ -0,0 +1,346 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/macOS/arch_macOS.h"
+#include "arch/macOS/net_serial.h"
+#include <termios.h>
+#include <sys/select.h>
+#include <IOKit/serial/ioss.h>
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _baudrate(0)
+    , _flags(0)
+    , serial_fd(-1)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
+{
+    if (isOpened()) close();
+    
+    serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+
+    if (serial_fd == -1) return false;
+
+    struct termios options, oldopt;
+    tcgetattr(serial_fd, &oldopt);
+	bzero(&options,sizeof(struct termios));
+
+    cfsetspeed(&options, B19200);
+
+	// enable rx and tx
+	options.c_cflag |= (CLOCAL | CREAD);
+
+
+    options.c_cflag &= ~PARENB; //no checkbit
+	options.c_cflag &= ~CSTOPB; //1bit stop bit
+
+	options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8; /* Select 8 data bits */
+
+#ifdef CNEW_RTSCTS
+    options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+    // raw input mode   
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    options.c_oflag &= ~OPOST;
+    
+    tcflush(serial_fd,TCIFLUSH); 
+
+    if (tcsetattr(serial_fd, TCSANOW, &options))
+    {
+        close();
+        return false;
+    }
+	
+    printf("Setting serial port baudrate...\n");
+    
+    speed_t speed = (speed_t)baudrate;
+    if (ioctl(serial_fd, IOSSIOSPEED, &speed)== -1) {
+        printf("Error calling ioctl(..., IOSSIOSPEED, ...) %s - %s(%d).\n",
+               portname, strerror(errno), errno);
+        close();
+        return false;
+    }
+
+    _is_serial_opened = true;
+
+    //Clear the DTR bit to let the motor spin
+    clearDTR();
+    
+    return true;
+}
+
+void raw_serial::close()
+{
+    if (serial_fd != -1)
+        ::close(serial_fd);
+    serial_fd = -1;
+    
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+// FIXME: non-block io should be used
+    if (!isOpened()) return 0;
+
+    if (data == NULL || size ==0) return 0;
+    
+    size_t tx_len = 0;
+    required_tx_cnt = 0;
+    do {
+        int ans = ::write(serial_fd, data + tx_len, size-tx_len);
+        
+        if (ans == -1) return tx_len;
+        
+        tx_len += ans;
+        required_tx_cnt = tx_len;
+    }while (tx_len<size);
+    
+    
+    return tx_len;
+}
+
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    
+    int ans = ::read(serial_fd, data, size);
+    
+    if (ans == -1) ans=0;
+    required_rx_cnt = ans;
+    return ans;
+}
+
+
+void raw_serial::flush( _u32 flags)
+{
+    tcflush(serial_fd,TCIFLUSH); 
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (returned_size) *returned_size = required_tx_cnt;
+    return 0;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+   
+    if (returned_size) *returned_size = required_rx_cnt;
+    return 0;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    size_t length = 0;
+    if (returned_size==NULL) returned_size=(size_t *)&length;
+    *returned_size = 0;
+    
+    int max_fd;
+    fd_set input_set;
+    struct timeval timeout_val;
+
+    /* Initialize the input set */
+    FD_ZERO(&input_set);
+    FD_SET(serial_fd, &input_set);
+    max_fd = serial_fd + 1;
+
+    /* Initialize the timeout structure */
+    timeout_val.tv_sec = timeout / 1000;
+    timeout_val.tv_usec = (timeout % 1000) * 1000;
+
+    if ( isOpened() )
+    {
+        int nread;
+
+        if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR;
+
+        *returned_size = nread;
+
+        if (*returned_size >= data_count)
+        {
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        /* Do the select */
+        int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
+
+        if (n < 0)
+        {
+            // select error
+            *returned_size =  0;
+            return ANS_DEV_ERR;
+        }
+        else if (n == 0)
+        {
+            // time out
+            *returned_size =0;
+            return ANS_TIMEOUT;
+        }
+        else
+        {
+            // data avaliable
+            assert (FD_ISSET(serial_fd, &input_set));
+
+
+            if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
+            if (*returned_size >= data_count)
+            {
+                return 0;
+            }
+        }
+        
+    }
+
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    size_t remaining;
+    
+    if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
+    return remaining;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIS, &dtr_bit);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIC, &dtr_bit);
+}
+
+void raw_serial::_init()
+{
+    serial_fd = 0;
+    _portName[0] = 0;
+    required_tx_cnt = required_rx_cnt = 0;
+}
+
+
+
+_u32 raw_serial::getTermBaudBitmap(_u32 baud)
+{
+#define BAUD_CONV(_baud_) case _baud_:  return _baud_
+    switch (baud)
+    {
+        BAUD_CONV(1200);
+        BAUD_CONV(1800);
+        BAUD_CONV(2400);
+        BAUD_CONV(4800);
+        BAUD_CONV(9600);
+        BAUD_CONV(19200);
+        BAUD_CONV(38400);
+        BAUD_CONV(57600);
+        BAUD_CONV(115200);
+        BAUD_CONV(230400);
+        BAUD_CONV(460800);
+        BAUD_CONV(500000);
+        BAUD_CONV(576000);
+        BAUD_CONV(921600);
+        BAUD_CONV(1000000);
+        BAUD_CONV(1152000);
+        BAUD_CONV(1500000);
+        BAUD_CONV(2000000);
+        BAUD_CONV(2500000);
+        BAUD_CONV(3000000);
+        BAUD_CONV(3500000);
+        BAUD_CONV(4000000);
+    }
+    return -1;
+}
+    
+}}} //end rp::arch::net
+
+
+
+//begin rp::hal
+namespace rp{ namespace hal{
+    
+    serial_rxtx * serial_rxtx::CreateRxTx()
+    {
+        return new rp::arch::net::raw_serial();
+    }
+    
+    void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
+    {
+        delete rxtx;
+    }
+    
+}} //end rp::hal
diff --git a/LIDAR/sdk/src/arch/macOS/net_serial.h b/LIDAR/sdk/src/arch/macOS/net_serial.h
new file mode 100644
index 0000000..41db813
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/net_serial.h
@@ -0,0 +1,84 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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 "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+    _u32 getTermBaudBitmap(_u32 baud);
+protected:
+    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    void _init();
+
+    char _portName[200];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    int serial_fd;
+
+    size_t required_tx_cnt;
+    size_t required_rx_cnt;
+};
+
+}}}
diff --git a/LIDAR/sdk/src/arch/macOS/net_socket.cpp b/LIDAR/sdk/src/arch/macOS/net_socket.cpp
new file mode 100644
index 0000000..2208fac
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/net_socket.cpp
@@ -0,0 +1,899 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2018 RoboPeak Project
+ *
+ *  macOS Implementation
+ */
+
+
+#include "sdkcommon.h"
+#include "../../hal/socket.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+namespace rp{ namespace net {
+
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{}
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, buffersize);
+        break;
+
+        case AF_INET6:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, buffersize);
+
+        break;
+    }
+    return !ans?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(in_addr_t)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(in6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_NOSIGPIPE, (char*)&bool_true, sizeof(bool_true));
+
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+
+        int ans;
+        if (pairAddress.getAddressType() == SocketAddress::ADDRESS_TYPE_INET) {
+            ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in));
+        } else {
+            ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in6));
+        }
+
+        if (!ans) return RESULT_OK;
+
+
+        switch (errno) {
+            case EAFNOSUPPORT:
+                return RESULT_OPERATION_NOT_SUPPORT;
+#if 0
+            case EINPROGRESS:
+                return RESULT_OK; //treat async connection as good status
+#endif
+            case ETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        size_t addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , (socklen_t*)&addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        size_t ans = ::send( _socket_fd, buffer, len, 0);
+        if (ans == (int)len) {
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        }
+        
+    }
+
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, 0);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+            
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+#if 0
+    virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <=  sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SHUT_RD;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SHUT_WR;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SHUT_RDWR;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+    int  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+        const struct sockaddr * addr = target ? reinterpret_cast<const struct sockaddr *>(target->getPlatformData()) : NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != -1) {
+            assert(ans == (int)len);
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+
+                case EMSGSIZE:
+                    return RESULT_INVALID_DATA;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        
+        }
+
+    }
+    virtual u_result setPairAddress(const SocketAddress* pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr);
+        int ans;
+        if (pairAddress->getAddressType() == SocketAddress::ADDRESS_TYPE_INET) {
+            ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in));
+        } else {
+            ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in6));
+        }   
+        return ans ? RESULT_OPERATION_FAIL : RESULT_OK;
+
+    }
+    
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+#if 0
+    virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
+
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+    
+protected:
+    int  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            assert(!"should not reach here, AF_PACKET is not supported on macOS");
+            return AF_INET;
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+
diff --git a/LIDAR/sdk/src/arch/macOS/thread.hpp b/LIDAR/sdk/src/arch/macOS/thread.hpp
new file mode 100644
index 0000000..72fd8f8
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/thread.hpp
@@ -0,0 +1,79 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/macOS/arch_macOS.h"
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+  //  return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+    return RESULT_OK;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+    // simply ignore this request
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}
diff --git a/LIDAR/sdk/src/arch/macOS/timer.cpp b/LIDAR/sdk/src/arch/macOS/timer.cpp
new file mode 100644
index 0000000..cae6e57
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/timer.cpp
@@ -0,0 +1,54 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/macOS/arch_macOS.h"
+
+
+namespace rp{ namespace arch{
+_u64 rp_getus()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000000LL + t.tv_nsec/1000;
+}
+_u64 rp_getms()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000L + t.tv_nsec/1000000L;
+}
+
+}}
diff --git a/LIDAR/sdk/src/arch/macOS/timer.h b/LIDAR/sdk/src/arch/macOS/timer.h
new file mode 100644
index 0000000..8f5ef04
--- /dev/null
+++ b/LIDAR/sdk/src/arch/macOS/timer.h
@@ -0,0 +1,58 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 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 "rptypes.h"
+
+#include <unistd.h>
+static inline void delay(_word_size_t ms){
+    while (ms>=1000){
+        usleep(1000*1000);
+        ms-=1000;
+    };
+    if (ms!=0)
+        usleep(ms*1000);
+}
+
+// TODO: the highest timer interface should be clock_gettime
+namespace rp{ namespace arch{
+
+_u64 rp_getus();
+_u64 rp_getms();
+
+}}
+
+#define getms() rp::arch::rp_getms()
+#define getus() rp::arch::rp_getus()
diff --git a/LIDAR/sdk/src/arch/win32/arch_win32.h b/LIDAR/sdk/src/arch/win32/arch_win32.h
new file mode 100644
index 0000000..3ae6655
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/arch_win32.h
@@ -0,0 +1,66 @@
+/*
+ *  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
+
+#pragma warning (disable: 4996)
+#define _CRT_SECURE_NO_WARNINGS
+
+#ifndef WINVER
+#define WINVER		0x0500
+#endif
+
+#ifndef _WIN32_WINNT
+#define _WIN32_WINNT	0x0501
+#endif
+
+
+#ifndef _WIN32_IE
+#define _WIN32_IE	0x0501
+#endif
+
+#ifndef _RICHEDIT_VER
+#define _RICHEDIT_VER	0x0200
+#endif
+
+
+#include <stddef.h>
+#include <stdio.h>
+#include <windows.h>
+#include <stdlib.h>   //for memcpy etc..
+#include <process.h>
+#include <direct.h> 
+
+
+#include "timer.h"
diff --git a/LIDAR/sdk/src/arch/win32/net_serial.cpp b/LIDAR/sdk/src/arch/win32/net_serial.cpp
new file mode 100644
index 0000000..f1fe025
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/net_serial.cpp
@@ -0,0 +1,367 @@
+/*
+ *  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.
+ *
+ */
+
+#include "sdkcommon.h"
+#include "net_serial.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _serial_handle(NULL)
+    , _baudrate(0)
+    , _flags(0)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+    CloseHandle(_ro.hEvent);
+    CloseHandle(_wo.hEvent);
+    CloseHandle(_wait_o.hEvent);
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags)
+{
+#ifdef _UNICODE
+    wchar_t wportname[1024];
+    mbstowcs(wportname, portname, sizeof(wportname) / sizeof(wchar_t));
+#endif
+
+    if (isOpened()) close();
+    
+    _serial_handle = CreateFile(
+#ifdef _UNICODE
+        wportname,
+#else
+        portname,
+#endif
+        GENERIC_READ | GENERIC_WRITE,
+        0,
+        NULL,
+        OPEN_EXISTING,
+        FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED,
+        NULL
+        );
+
+    if (_serial_handle == INVALID_HANDLE_VALUE) return false;
+
+    if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE))
+    {
+        close();
+        return false;
+    }
+    
+    _dcb.BaudRate = baudrate;
+    _dcb.ByteSize = 8;
+    _dcb.Parity   = NOPARITY;
+    _dcb.StopBits = ONESTOPBIT;
+    _dcb.fDtrControl = DTR_CONTROL_ENABLE;
+
+    if (!SetCommState(_serial_handle, &_dcb))
+    {
+        close();
+        return false;
+    }
+
+    if (!SetCommTimeouts(_serial_handle, &_co))
+    {
+        close();
+        return false;
+    }
+
+    if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ))
+    {
+        close();
+        return false;
+    }
+
+    if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ))
+    {
+        close();
+        return false;
+    }
+
+    Sleep(30); 
+    _is_serial_opened = true;
+
+    //Clear the DTR bit set DTR=high
+    clearDTR();
+
+    return true;
+}
+
+void raw_serial::close()
+{
+    SetCommMask(_serial_handle, 0);
+    ResetEvent(_wait_o.hEvent);
+
+    CloseHandle(_serial_handle);
+    _serial_handle = INVALID_HANDLE_VALUE;
+    
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+    DWORD    error;
+    DWORD w_len = 0, o_len = -1;
+    if (!isOpened()) return ANS_DEV_ERR;
+
+    if (data == NULL || size ==0) return 0;
+    
+    if(ClearCommError(_serial_handle, &error, NULL) && error > 0)
+        PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR);
+
+    if(!WriteFile(_serial_handle, data, (DWORD)size, &w_len, &_wo))
+        if(GetLastError() != ERROR_IO_PENDING)
+            w_len = ANS_DEV_ERR;
+
+    return w_len;
+}
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    DWORD r_len = 0;
+
+
+    if(!ReadFile(_serial_handle, data, (DWORD)size, &r_len, &_ro))
+    {
+        if(GetLastError() == ERROR_IO_PENDING) 
+        {
+            if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE))
+            {
+                if(GetLastError() != ERROR_IO_INCOMPLETE)
+                    r_len = 0;
+            }
+        }
+        else
+            r_len = 0;
+    }
+
+    return r_len;
+}
+
+void raw_serial::flush( _u32 flags)
+{
+    PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR );
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return ANS_DEV_ERR;
+    DWORD w_len = 0;
+    _word_size_t ans =0;
+
+    if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT)
+    {
+        ans = ANS_TIMEOUT;
+        goto _final;
+    }
+    if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE))
+    {
+        ans = ANS_DEV_ERR;
+    }
+_final:
+    if (returned_size) *returned_size = w_len;
+    return (int)ans;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+    DWORD r_len = 0;
+    _word_size_t ans =0;
+
+    if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT)
+    {
+        ans = ANS_TIMEOUT;
+    }
+    if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE))
+    {
+        ans = ANS_DEV_ERR;
+    }
+    if (returned_size) *returned_size = r_len;
+    return (int)ans;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    COMSTAT  stat;
+    DWORD error;
+    DWORD msk,length;
+    size_t dummy_length;
+
+    if (returned_size==NULL) returned_size=(size_t *)&dummy_length;
+
+    
+    if ( isOpened()) {
+        size_t rxqueue_remaining =  rxqueue_count();
+        if (rxqueue_remaining >= data_count) {
+            *returned_size = rxqueue_remaining;
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        msk = 0;
+        SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR );
+        if(!WaitCommEvent(_serial_handle, &msk, &_wait_o))
+        {
+            if(GetLastError() == ERROR_IO_PENDING)
+            {
+                if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT)
+                {
+                    *returned_size =0;
+                    return ANS_TIMEOUT;
+                }
+
+                GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE);
+
+                ::ResetEvent(_wait_o.hEvent);
+            }else
+            {
+                ClearCommError(_serial_handle, &error, &stat);
+                 *returned_size = stat.cbInQue;
+                return ANS_DEV_ERR;
+            }
+        }
+
+        if(msk & EV_ERR){
+            // FIXME: may cause problem here
+            ClearCommError(_serial_handle, &error, &stat);
+        }
+
+        if(msk & EV_RXCHAR){
+            ClearCommError(_serial_handle, &error, &stat);
+            if(stat.cbInQue >= data_count)
+            {
+                *returned_size = stat.cbInQue;
+                return 0;
+            }
+        }
+    }
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    COMSTAT  com_stat;
+    DWORD error;
+    DWORD r_len = 0;
+
+    if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0)
+    {
+        PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR);
+        return 0;
+    }
+    return com_stat.cbInQue;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    EscapeCommFunction(_serial_handle, SETDTR);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    EscapeCommFunction(_serial_handle, CLRDTR);
+}
+
+
+void raw_serial::_init()
+{
+    memset(&_dcb, 0, sizeof(_dcb));
+    _dcb.DCBlength = sizeof(_dcb);
+    _serial_handle = INVALID_HANDLE_VALUE;
+    memset(&_co, 0, sizeof(_co));
+    _co.ReadIntervalTimeout = 0;
+    _co.ReadTotalTimeoutMultiplier = 0;
+    _co.ReadTotalTimeoutConstant = 0;
+    _co.WriteTotalTimeoutMultiplier = 0;
+    _co.WriteTotalTimeoutConstant = 0;
+
+    memset(&_ro, 0, sizeof(_ro));
+    memset(&_wo, 0, sizeof(_wo));
+    memset(&_wait_o, 0, sizeof(_wait_o));
+
+    _ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
+    _wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
+    _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
+
+    _portName[0] = 0;
+}
+
+}}} //end rp::arch::net
+
+
+//begin rp::hal
+namespace rp{ namespace hal{
+
+serial_rxtx * serial_rxtx::CreateRxTx()
+{
+    return new rp::arch::net::raw_serial();
+}
+
+void  serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx)
+{
+    delete rxtx;
+}
+
+
+}} //end rp::hal
diff --git a/LIDAR/sdk/src/arch/win32/net_serial.h b/LIDAR/sdk/src/arch/win32/net_serial.h
new file mode 100644
index 0000000..d43137e
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/net_serial.h
@@ -0,0 +1,86 @@
+/*
+ *  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 "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+        SERIAL_RX_TIMEOUT     = 2000,
+        SERIAL_TX_TIMEOUT     = 2000,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+protected:
+    bool open(const char * portname, _u32 baudrate, _u32 flags);
+    void _init();
+
+    char _portName[20];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    OVERLAPPED _ro, _wo;
+    OVERLAPPED _wait_o;
+    volatile HANDLE _serial_handle;
+    DCB _dcb;
+    COMMTIMEOUTS _co;
+};
+
+}}}
diff --git a/LIDAR/sdk/src/arch/win32/net_socket.cpp b/LIDAR/sdk/src/arch/win32/net_socket.cpp
new file mode 100644
index 0000000..611c9d1
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/net_socket.cpp
@@ -0,0 +1,945 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ *
+ *  Win32 Implementation
+ */
+
+#define _WINSOCKAPI_
+
+#include "sdkcommon.h"
+#include "..\..\hal\socket.h"
+#include <windows.h>  
+#include <winsock2.h>
+#include <ws2tcpip.h>
+
+#include <stdlib.h>  
+#include <stdio.h>  
+#pragma comment (lib, "Ws2_32.lib")
+
+namespace rp{ namespace net {
+
+static volatile bool _isWSAStartupCalled = false;
+
+static inline bool _checkWSAStartup() {
+    int  iResult;
+    WSADATA wsaData;
+    if (!_isWSAStartupCalled) {
+        iResult = WSAStartup(MAKEWORD(2,2), &wsaData);
+        if (iResult != 0) {
+            return false;
+        }
+        _isWSAStartupCalled = true;
+    }
+    return true;
+}
+
+static const char* _inet_ntop(int af, const void* src, char* dst, int cnt){
+
+	struct sockaddr_storage srcaddr;
+
+
+    memset(dst, 0, cnt);
+
+	memset(&srcaddr, 0, sizeof(struct sockaddr_storage));
+	
+
+    srcaddr.ss_family = af;
+
+    switch (af) {
+    case AF_INET:
+        {
+            struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&srcaddr);
+            memcpy(&(ipv4->sin_addr), src, sizeof(ipv4->sin_addr));
+        }
+        break;
+    case AF_INET6:
+        {
+            struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&srcaddr);
+            memcpy(&(ipv6->sin6_addr), src, sizeof(ipv6->sin6_addr));
+        }
+        break;
+    }
+
+	if (WSAAddressToStringA((struct sockaddr*) &srcaddr, sizeof(struct sockaddr_storage), 0, dst, (LPDWORD) &cnt) != 0) {
+		DWORD rv = WSAGetLastError();
+		return NULL;
+	}
+	return dst;
+}
+
+static int _inet_pton(int Family, const char * pszAddrString, void* pAddrBuf)
+{
+    struct sockaddr_storage tmpholder;
+    int actualSize =  sizeof(sockaddr_storage);
+
+    int result = WSAStringToAddressA((char *)pszAddrString, Family, NULL, (sockaddr*)&tmpholder, &actualSize);
+    if (result) return -1;
+
+    switch (Family) {
+        case AF_INET:
+            {
+                struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&tmpholder);
+                memcpy(pAddrBuf, &(ipv4->sin_addr), sizeof(ipv4->sin_addr));
+            }
+            break;
+        case AF_INET6:
+            {
+                struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&tmpholder);
+                memcpy(pAddrBuf, &(ipv6->sin6_addr), sizeof(ipv6->sin6_addr));
+            }
+            break;
+    }
+    return 1;
+}
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _checkWSAStartup();
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _checkWSAStartup();
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{ _checkWSAStartup(); }
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = _inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = _inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, (int)buffersize);
+        break;
+
+        case AF_INET6:
+            ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, (int)buffersize);
+
+        break;
+    }
+    return (ans==NULL)?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+    _checkWSAStartup();
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(SOCKET fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, (int)sizeof(bool_true) );
+        
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        closesocket(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        int actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, &actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, (int)sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv, (int)sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        u_long mode_block = 0;
+        u_long mode_notBlock = 1;
+        
+        //set to non block mode
+        if (SOCKET_ERROR == ioctlsocket(_socket_fd, (long)FIONBIO, &mode_notBlock))
+        {
+            return RESULT_OPERATION_FAIL;
+        }
+
+        struct timeval tm;  
+        tm.tv_sec  = 2;  
+        tm.tv_usec = 0;  
+        int ret = -1;  
+
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        if (!ans) return RESULT_OK;
+
+        fd_set set;  
+        FD_ZERO(&set);  
+        FD_SET(_socket_fd, &set);
+
+        if (select(-1, NULL, &set, NULL, &tm) <= 0)  
+        {  
+            ret = -1; // error(select error or timeout)  
+            return RESULT_OPERATION_TIMEOUT;
+        }  
+        
+        int error = -1;  
+        int optLen = sizeof(int);  
+        getsockopt(_socket_fd, SOL_SOCKET, SO_ERROR, (char*)&error, &optLen);   
+
+        if (0 != error)  
+        {  
+            ret = -1; // error  
+        }  
+        else  
+        {  
+            ret = 1;  // correct  
+        }  
+
+        //set back to block mode
+        if (SOCKET_ERROR == ioctlsocket(_socket_fd, (long)FIONBIO, &mode_block))
+        {
+            return RESULT_OPERATION_FAIL;
+        }
+        if(1 == ret)  
+        {
+            return RESULT_OK;
+        }
+        else
+        {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        int addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        SOCKET pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , &addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        int ans = ::send( _socket_fd, (const char *)buffer, (int)len, 0);
+        if (ans != SOCKET_ERROR ) {
+            assert(ans == (int)len);
+
+            return RESULT_OK;
+        } else {
+            switch(WSAGetLastError()) {
+            case WSAETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+            }
+            
+        }
+        
+    }
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        int ans = ::recv( _socket_fd, (char *)buf, (int)len, 0);
+		//::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK,  (const char *)1, sizeof(int));
+		//::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int))
+        if (ans == SOCKET_ERROR) {
+            recv_len = 0;  
+                switch(WSAGetLastError()) {
+                case WSAETIMEDOUT:
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+                }
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        int actualsize =  (int)sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, &actualsize);
+
+        assert(actualsize <=  (int)sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SD_RECEIVE;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SD_SEND;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SD_BOTH;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY, (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(NULL, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select((int)_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+
+    SOCKET  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(SOCKET fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, (int)sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        closesocket(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        int actualsize =  (int)sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, &actualsize);
+
+        assert(actualsize <= (int)sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, (int)sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&tv, (int)sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(NULL, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(NULL, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result setPairAddress(const SocketAddress * pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr * addr = pairAddress ? reinterpret_cast<const struct sockaddr *>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr *>(&unspecAddr);
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        return ans? RESULT_OPERATION_FAIL: RESULT_OK;
+
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+
+        const struct sockaddr * addr = target?reinterpret_cast<const struct sockaddr *>(target->getPlatformData()): NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != SOCKET_ERROR) {
+            assert(ans == (int)len);
+            return RESULT_OK;
+        } else {
+           switch(WSAGetLastError()) {
+            case WSAETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            case WSAEMSGSIZE:
+                return RESULT_INVALID_DATA;
+            default:
+                return RESULT_OPERATION_FAIL;
+            }
+        }
+
+    }
+
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        int source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        int ans = ::recvfrom( _socket_fd, (char *)buf, (int)len, 0, addr, addr?&source_addr_size:NULL);
+        if (ans == SOCKET_ERROR) {
+            recv_len = 0;  
+            int errCode = WSAGetLastError();
+           switch(errCode) {
+            case WSAETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+            }
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+
+    
+protected:
+    SOCKET  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            return AF_UNSPEC; //win32 doesn't support RAW Packet
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    _checkWSAStartup();
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    SOCKET socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    _checkWSAStartup();
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    SOCKET socket_fd = ::socket(socket_family, (family == SOCKET_FAMILY_RAW) ? SOCK_RAW : SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+
diff --git a/LIDAR/sdk/src/arch/win32/timer.cpp b/LIDAR/sdk/src/arch/win32/timer.cpp
new file mode 100644
index 0000000..d153cc4
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/timer.cpp
@@ -0,0 +1,72 @@
+/*
+ *  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.
+ *
+ */
+
+#include "sdkcommon.h"
+#include <mmsystem.h>
+#pragma comment(lib, "Winmm.lib")
+
+namespace rp{ namespace arch{
+
+static LARGE_INTEGER _current_freq;
+
+void HPtimer_reset()
+{
+    BOOL ans=QueryPerformanceFrequency(&_current_freq);
+    _current_freq.QuadPart/=1000ULL;
+    assert(ans);
+}
+
+_u64 getHDTimer_us()
+{
+    LARGE_INTEGER current;
+    QueryPerformanceCounter(&current);
+
+    return (_u64)(current.QuadPart / (_current_freq.QuadPart/1000ULL));
+
+}
+
+_u64 getHDTimer()
+{
+    LARGE_INTEGER current;
+    QueryPerformanceCounter(&current);
+
+    return (_u64)(current.QuadPart/_current_freq.QuadPart);
+}
+
+BEGIN_STATIC_CODE(timer_cailb)
+{
+    HPtimer_reset();
+}END_STATIC_CODE(timer_cailb)
+
+}}
diff --git a/LIDAR/sdk/src/arch/win32/timer.h b/LIDAR/sdk/src/arch/win32/timer.h
new file mode 100644
index 0000000..27b7a76
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/timer.h
@@ -0,0 +1,49 @@
+/*
+ *  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 "hal/types.h"
+
+#define delay(x)   ::Sleep(x)
+
+namespace rp{ namespace arch{
+    void HPtimer_reset();
+    _u64 getHDTimer();
+    _u64 getHDTimer_us();
+
+}}
+
+#define getms()   rp::arch::getHDTimer()
+#define getus()   rp::arch::getHDTimer_us()
diff --git a/LIDAR/sdk/src/arch/win32/winthread.hpp b/LIDAR/sdk/src/arch/win32/winthread.hpp
new file mode 100644
index 0000000..590794d
--- /dev/null
+++ b/LIDAR/sdk/src/arch/win32/winthread.hpp
@@ -0,0 +1,144 @@
+/*
+ *  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.
+ *
+ */
+
+#include "sdkcommon.h"
+#include <process.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+
+    newborn._handle = (_word_size_t)( 
+        _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc,
+                        data, 0, NULL));
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    if (TerminateThread( reinterpret_cast<HANDLE>(this->_handle), -1))
+    {
+        CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
+        this->_handle = NULL;
+        return RESULT_OK;
+    }else
+    {
+        return RESULT_OPERATION_FAIL;
+    }
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+    HANDLE selfHandle = GetCurrentThread();
+	
+	int win_priority =  THREAD_PRIORITY_NORMAL;
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		win_priority = THREAD_PRIORITY_TIME_CRITICAL;
+		break;
+	case PRIORITY_HIGH:
+		win_priority = THREAD_PRIORITY_HIGHEST;
+		break;
+	case PRIORITY_NORMAL:
+		win_priority = THREAD_PRIORITY_NORMAL;
+		break;
+	case PRIORITY_LOW:
+		win_priority = THREAD_PRIORITY_LOWEST;
+		break;
+	case PRIORITY_IDLE:
+		win_priority = THREAD_PRIORITY_IDLE;
+		break;
+	}
+
+	if (SetThreadPriority(selfHandle, win_priority))
+	{
+		return RESULT_OK;
+	}
+	return RESULT_OPERATION_FAIL;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+	int win_priority =  ::GetThreadPriority(reinterpret_cast<HANDLE>(this->_handle));
+	
+	if (win_priority == THREAD_PRIORITY_ERROR_RETURN)
+	{
+		return PRIORITY_NORMAL;
+	}
+
+	if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL )
+	{
+		return PRIORITY_REALTIME;
+	}
+	else if (win_priority<THREAD_PRIORITY_TIME_CRITICAL && win_priority>=THREAD_PRIORITY_ABOVE_NORMAL)
+	{	
+		return PRIORITY_HIGH;
+	}
+	else if (win_priority<THREAD_PRIORITY_ABOVE_NORMAL && win_priority>THREAD_PRIORITY_BELOW_NORMAL)
+	{
+		return PRIORITY_NORMAL;
+	}else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE)
+	{
+		return PRIORITY_LOW;
+	}else if (win_priority<=THREAD_PRIORITY_IDLE)
+	{
+		return PRIORITY_IDLE;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    switch ( WaitForSingleObject(reinterpret_cast<HANDLE>(this->_handle), timeout))
+    {
+    case WAIT_OBJECT_0:
+        CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
+        this->_handle = NULL;
+        return RESULT_OK;
+    case WAIT_ABANDONED:
+        return RESULT_OPERATION_FAIL;
+    case WAIT_TIMEOUT:
+        return RESULT_OPERATION_TIMEOUT;
+    }
+
+    return RESULT_OK;
+}
+
+}}
diff --git a/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h b/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h
new file mode 100644
index 0000000..70b9c99
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h
@@ -0,0 +1,60 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  External Reference and dependencies
+  */
+
+  /*
+	* 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 "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "hal/waiter.h"
+#include "hal/byteorder.h"
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+#include <memory>
+
+#define CONF_NO_BOOST_CRC_SUPPORT
+
+#include "dataupacker_namespace.h"
+
+
diff --git a/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h b/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h
new file mode 100644
index 0000000..6b1b6dd
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h
@@ -0,0 +1,74 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Internal Definition
+  */
+
+  /*
+	* 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
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker
+{
+public:
+	LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0;
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0;
+	virtual void publishNewScanReset() = 0;
+
+
+	virtual _u64 getCurrentTimestamp_uS() = 0;
+
+};
+
+class IDataUnpackerHandler
+{
+public:
+	IDataUnpackerHandler() {}
+	virtual ~IDataUnpackerHandler() {}
+
+
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual _u8 getSampleAnswerType() const = 0;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0;
+	virtual void reset() = 0;
+
+};
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp b/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp
new file mode 100644
index 0000000..74680c0
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp
@@ -0,0 +1,259 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "dataunnpacker_commondef.h"
+#include "dataunpacker.h"
+#include "dataunnpacker_internal.h"
+
+
+#include <map>
+
+
+#define REGISTER_HANDLER(_c_) {     \
+		auto newBorn = new unpacker::_c_();   \
+		if (!newBorn) return false; \
+		handlerList.push_back(newBorn); \
+	} 
+
+// How to include new handlers?
+// 1. add extra include line below if a new handle is to be included
+// 2. update the code in function _registerDataUnpackerHandlers
+#include "unpacker/handler_capsules.h"
+#include "unpacker/handler_hqnode.h"
+#include "unpacker/handler_normalnode.h"
+
+
+#define  DEF_REGISTER_HANDLER_LIST
+
+
+BEGIN_DATAUNPACKER_NS()
+
+
+static bool _registerDataUnpackerHandlers(std::vector<IDataUnpackerHandler *> & handlerList)
+{
+	REGISTER_HANDLER(UnpackerHandler_NormalNode);
+	REGISTER_HANDLER(UnpackerHandler_HQNode);
+	REGISTER_HANDLER(UnpackerHandler_CapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode);
+	return true;
+}
+
+
+class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner
+{
+public:
+
+	void registerHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_handlerMap[ansType] = handler;
+	}
+
+
+	void unregisterAllHandlers()
+	{
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			delete itr->second;
+		}
+		_handlerMap.clear();
+	}
+
+	LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l)
+		: LIDARSampleDataUnpackerInner(l)
+		, _enabled(false)
+		, _lastActiveAnsType(0)
+		, _lastActiveHandler(nullptr)
+	{
+
+	}
+
+	virtual ~LIDARSampleDataUnpackerImpl()
+	{
+		unregisterAllHandlers();
+	}
+
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size)
+	{
+	
+		// notify the handlers ...
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			itr->second->onUnpackerContextSet(type, data, size);
+		}
+	}
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) {
+		if (!_enabled) return false;
+
+
+		if (_lastActiveAnsType != ansType) {
+			onDeselectHandler();
+
+			auto itr = _handlerMap.find(ansType);
+			if (itr != _handlerMap.end()) {
+				onSelectHandler(ansType, itr->second);
+			}
+			else {
+				onSelectHandler(ansType, nullptr);
+			}
+			
+		}
+
+		if (_lastActiveHandler) {
+			_lastActiveHandler->onData(this, reinterpret_cast<const _u8 *>(buffer), size);
+			return true;
+		}
+		else {
+			return false;
+		}
+	}
+
+	virtual void reset()
+	{
+		clearCache();
+		_lastActiveHandler = nullptr;
+		_lastActiveAnsType = 0;
+
+	}
+
+	virtual void enable()
+	{
+		_enabled = true;
+		reset();
+	}
+
+	virtual void disable()
+	{
+		_enabled = false;
+		reset();
+
+	}
+
+	virtual void clearCache()
+	{
+		if (_lastActiveHandler) {
+			_lastActiveHandler->reset();
+		}
+	}
+
+	virtual _u64 getCurrentTimestamp_uS() {
+		return getus();
+	}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node)
+	{
+		_listener.onHQNodeDecoded(timestamp_uS, node);
+	}
+
+
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size)
+	{
+		_listener.onDecodingError(errorType, ansType, payload, size);
+
+	}
+
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size)
+	{
+		_listener.onCustomSampleDataDecoded(ansType, customCode, payload, size);
+	}
+
+
+	virtual void publishNewScanReset()
+	{
+		_listener.onHQNodeScanResetReq();
+	}
+protected:
+
+	void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_lastActiveHandler = handler;
+		_lastActiveAnsType = ansType;
+	}
+
+	void onDeselectHandler()
+	{
+		reset();
+	}
+
+
+protected:
+	bool _enabled;
+	std::map<_u8, IDataUnpackerHandler*> _handlerMap;
+
+	_u8 _lastActiveAnsType;
+	IDataUnpackerHandler* _lastActiveHandler;
+};
+
+LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener)
+{
+	LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener);
+	
+	std::vector<IDataUnpackerHandler*> list;
+	if (!_registerDataUnpackerHandlers(list)) {
+		delete  impl;
+		for (auto itr = list.begin(); itr != list.end(); ++itr) {
+			delete* itr;
+		}
+		impl = nullptr;
+	}
+
+	for (auto itr = list.begin(); itr != list.end(); ++itr) {
+		impl->registerHandler((*itr)->getSampleAnswerType(), (*itr));
+	}
+	return impl;
+}
+
+void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) {
+	delete unpacker;
+}
+
+LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() {
+
+}
+
+LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l) 
+	: _listener(l)
+{
+
+}
+
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/dataunpacker.h b/LIDAR/sdk/src/dataunpacker/dataunpacker.h
new file mode 100644
index 0000000..2cdf6ca
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/dataunpacker.h
@@ -0,0 +1,93 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* 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 "dataupacker_namespace.h"
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataListener
+{
+
+
+public:
+	virtual void onHQNodeScanResetReq() = 0;
+	virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {}
+
+	virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {}
+};
+
+class LIDARSampleDataUnpacker
+{
+public:
+	enum {
+		ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001,
+		ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002,
+	};
+
+	enum UnpackerContextType {
+		UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0,
+		UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1,
+		UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2,
+
+	};
+
+	virtual ~LIDARSampleDataUnpacker();
+	static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener);
+	static void ReleaseInstance(LIDARSampleDataUnpacker*);
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual void enable() = 0;
+	virtual void disable() = 0;
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0;
+	virtual void reset() = 0;
+	virtual void clearCache() = 0;
+
+protected:
+	LIDARSampleDataUnpacker(LIDARSampleDataListener&);
+	LIDARSampleDataListener& _listener;
+
+};
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h b/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h
new file mode 100644
index 0000000..9e27192
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h
@@ -0,0 +1,5 @@
+#pragma once
+
+
+#define BEGIN_DATAUNPACKER_NS()  namespace sl{ namespace internal{
+#define END_DATAUNPACKER_NS()  }}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp
new file mode 100644
index 0000000..177c214
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp
@@ -0,0 +1,1054 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Capsule Style Sample Node Handlers
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+
+
+#include "handler_capsules.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+// UnpackerHandler_CapsuleNode
+///////////////////////////////////////////////////////////////////////////////////
+
+static _u64 _getSampleDelayOffsetInExpressMode(const SlamtecLidarTimingDesc& timing, int sampleIdx)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_capsule_measurement_nodes_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+    const _u64 groupingDelay = (31 - sampleIdx) * timing.sample_duration_uS;
+
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay;
+}
+
+
+UnpackerHandler_CapsuleNode::UnpackerHandler_CapsuleNode()
+    : _cached_scan_node_buf_pos(0)
+    , _is_previous_capsuledataRdy(false)
+    , _cached_last_data_timestamp_us(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_CapsuleNode::~UnpackerHandler_CapsuleNode()
+{
+
+}
+
+void UnpackerHandler_CapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+
+_u8 UnpackerHandler_CapsuleNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
+}
+
+void UnpackerHandler_CapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit 1
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) {
+                // pass
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the sync bit 2
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_capsule_measurement_nodes_t* node = reinterpret_cast<rplidar_response_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+            // calc the checksum ...
+            _u8 checksum = 0;
+            _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4));
+            for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6);
+                cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos)
+            {
+                checksum ^= _cached_scan_node_buf[cpos];
+            }
+
+            if (recvChecksum == checksum)
+            {
+                // only consider vaild if the checksum matches...
+
+                // perform data endianess convertion if necessary
+#ifdef _CPU_ENDIAN_BIG
+                node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6);
+                for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) {
+                    node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1);
+                    node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2);
+                }
+#endif
+                if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT)
+                {
+                    if (_is_previous_capsuledataRdy) {
+                        engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET
+                            , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node));
+                    }
+                    // this is the first capsule frame in logic, discard the previous cached data...
+                    _is_previous_capsuledataRdy = false;
+                    engine->publishNewScanReset();
+
+
+                }
+                _onScanNodeCapsuleData(*node, engine);
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+
+
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node));
+
+            }
+            continue;
+        }
+        break;
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+void UnpackerHandler_CapsuleNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+    _is_previous_capsuledataRdy = false;
+    _cached_last_data_timestamp_us = 0;
+}
+
+void UnpackerHandler_CapsuleNode::_onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine)
+{
+    _u64 currentTS = engine->getCurrentTimestamp_uS();
+    if (_is_previous_capsuledataRdy) {
+        int diffAngle_q8;
+        int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
+        int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
+
+        diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8);
+        if (prevStartAngle_q8 > currentStartAngle_q8) {
+            diffAngle_q8 += (360 << 8);
+        }
+
+        int angleInc_q16 = (diffAngle_q8 << 3);
+        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
+        for (int pos = 0; pos < (int)_countof(_cached_previous_capsuledata.cabins); ++pos)
+        {
+            int dist_q2[2];
+            int angle_q6[2];
+            int syncBit[2];
+
+            dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC);
+            dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC);
+
+            int angle_offset1_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3) << 4));
+            int angle_offset2_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3) << 4));
+
+            angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10);
+            syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
+            currentAngle_raw_q16 += angleInc_q16;
+
+
+            angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10);
+            syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
+            currentAngle_raw_q16 += angleInc_q16;
+
+            for (int cpos = 0; cpos < 2; ++cpos) {
+
+                if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
+                if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
+
+                rplidar_response_measurement_node_hq_t hqNode;
+
+
+                hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
+                hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
+
+                hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90;
+                hqNode.dist_mm_q2 = dist_q2[cpos];
+
+                engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInExpressMode(_cachedTimingDesc, pos * 2 + cpos), &hqNode);
+            }
+
+        }
+    }
+
+    _cached_previous_capsuledata = capsule;
+    _is_previous_capsuledataRdy = true;
+    _cached_last_data_timestamp_us = currentTS;
+
+}
+
+
+// UnpackerHandler_UltraCapsuleNode
+///////////////////////////////////////////////////////////////////////////////////
+
+static _u64 _getSampleDelayOffsetInUltraBoostMode(const SlamtecLidarTimingDesc& timing, int sampleIdx)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+    const _u64 groupingDelay = ((32 * 3 - 1) - sampleIdx) * timing.sample_duration_uS;
+
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay;
+}
+
+
+UnpackerHandler_UltraCapsuleNode::UnpackerHandler_UltraCapsuleNode()
+    : _cached_scan_node_buf_pos(0)
+    , _is_previous_capsuledataRdy(false)
+    , _cached_last_data_timestamp_us(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_UltraCapsuleNode::~UnpackerHandler_UltraCapsuleNode()
+{
+
+}
+
+void UnpackerHandler_UltraCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+
+_u8 UnpackerHandler_UltraCapsuleNode::getSampleAnswerType() const
+{
+    return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA;
+}
+
+void UnpackerHandler_UltraCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit 1
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) {
+                // pass
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the sync bit 2
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_ultra_capsule_measurement_nodes_t* node = reinterpret_cast<rplidar_response_ultra_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+            // calc the checksum ...
+            _u8 checksum = 0;
+            _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4));
+            for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6);
+                cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos)
+            {
+                checksum ^= _cached_scan_node_buf[cpos];
+            }
+
+            if (recvChecksum == checksum)
+            {
+                // only consider vaild if the checksum matches...
+
+                // perform data endianess convertion if necessary
+#ifdef _CPU_ENDIAN_BIG
+                node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6);
+                for (size_t cpos = 0; cpos < _countof(node->ultra_cabins); ++cpos) {
+                    node->ultra_cabins[cpos].combined_x3 = le32_to_cpu(node->ultra_cabins[cpos].combined_x3);
+                }
+#endif
+                if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT)
+                {
+                    if (_is_previous_capsuledataRdy) {
+                        engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET
+                            , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node));
+
+                    }
+                    // this is the first capsule frame in logic, discard the previous cached data...
+                    _is_previous_capsuledataRdy = false;
+
+                    engine->publishNewScanReset();
+
+                }
+                _onScanNodeUltraCapsuleData(*node, engine);
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node));
+
+            }
+            continue;
+        }
+        break;
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+void UnpackerHandler_UltraCapsuleNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+    _is_previous_capsuledataRdy = false;
+}
+
+static _u32 _varbitscale_decode(_u32 scaled, _u32& scaleLevel)
+{
+    static const _u32 VBS_SCALED_BASE[] = {
+        RPLIDAR_VARBITSCALE_X16_DEST_VAL,
+        RPLIDAR_VARBITSCALE_X8_DEST_VAL,
+        RPLIDAR_VARBITSCALE_X4_DEST_VAL,
+        RPLIDAR_VARBITSCALE_X2_DEST_VAL,
+        0,
+    };
+
+    static const _u32 VBS_SCALED_LVL[] = {
+        4,
+        3,
+        2,
+        1,
+        0,
+    };
+
+    static const _u32 VBS_TARGET_BASE[] = {
+        (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT),
+        (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT),
+        (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT),
+        (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT),
+        0,
+    };
+
+    for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i)
+    {
+        int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]);
+        if (remain >= 0) {
+            scaleLevel = VBS_SCALED_LVL[i];
+            return VBS_TARGET_BASE[i] + (remain << scaleLevel);
+        }
+    }
+
+    return 0;
+}
+
+void UnpackerHandler_UltraCapsuleNode::_onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine)
+{
+    _u64 currentTS = engine->getCurrentTimestamp_uS();
+    if (_is_previous_capsuledataRdy) {
+        int diffAngle_q8;
+        int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
+        int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
+
+        diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8);
+        if (prevStartAngle_q8 > currentStartAngle_q8) {
+            diffAngle_q8 += (360 << 8);
+        }
+
+        int angleInc_q16 = (diffAngle_q8 << 3) / 3;
+        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
+        for (int pos = 0; pos < (int)_countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos)
+        {
+            int dist_q2[3];
+            int angle_q6[3];
+            int syncBit[3];
+
+
+            _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3;
+
+            // unpack ...
+            int dist_major = (combined_x3 & 0xFFF);
+
+            // signed partical integer, using the magic shift here
+            // DO NOT TOUCH
+
+            int dist_predict1 = (((int)(combined_x3 << 10)) >> 22);
+            int dist_predict2 = (((int)combined_x3) >> 22);
+
+            int dist_major2;
+
+            _u32 scalelvl1=0, scalelvl2 = 0;
+
+            // prefetch next ...
+            if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1)
+            {
+                dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF);
+            }
+            else {
+                dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF);
+            }
+
+            // decode with the var bit scale ...
+            dist_major = _varbitscale_decode(dist_major, scalelvl1);
+            dist_major2 = _varbitscale_decode(dist_major2, scalelvl2);
+
+
+            int dist_base1 = dist_major;
+            int dist_base2 = dist_major2;
+
+            if ((!dist_major) && dist_major2) {
+                dist_base1 = dist_major2;
+                scalelvl1 = scalelvl2;
+            }
+
+
+            dist_q2[0] = (dist_major << 2);
+            if (((_u32)dist_predict1 == 0xFFFFFE00) || ((_u32)dist_predict1 == 0x1FF)) {
+                dist_q2[1] = 0;
+            }
+            else {
+                dist_predict1 = (int)(dist_predict1 << scalelvl1);
+                dist_q2[1] = (dist_predict1 + dist_base1) << 2;
+
+            }
+
+            if (((_u32)dist_predict2 == 0xFFFFFE00) || ((_u32)dist_predict2 == 0x1FF)) {
+                dist_q2[2] = 0;
+            }
+            else {
+                dist_predict2 = (int)(dist_predict2 << scalelvl2);
+                dist_q2[2] = (dist_predict2 + dist_base2) << 2;
+            }
+
+            for (int cpos = 0; cpos < 3; ++cpos)
+            {
+
+                syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
+
+
+                rplidar_response_measurement_node_hq_t hqNode;
+
+
+                int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0);
+
+                if (dist_q2[cpos] >= (50 * 4))
+                {
+                    const int k1 = 98361;
+                    const int k2 = int(k1 / dist_q2[cpos]);
+
+                    offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304;
+                }
+
+                angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10);
+                currentAngle_raw_q16 += angleInc_q16;
+
+                if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
+                if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
+
+
+                hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
+                hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
+
+                hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90;
+                hqNode.dist_mm_q2 = dist_q2[cpos];
+
+                engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInUltraBoostMode(_cachedTimingDesc, pos * 3 + cpos), &hqNode);
+            }
+
+        }
+    }
+
+    _cached_previous_ultracapsuledata = capsule;
+    _is_previous_capsuledataRdy = true;
+    _cached_last_data_timestamp_us = currentTS;
+
+}
+
+
+// UnpackerHandler_DenseCapsuleNode
+///////////////////////////////////////////////////////////////////////////////////
+
+static _u64 _getSampleDelayOffsetInDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+    const _u64 groupingDelay = (39 - sampleIdx) * timing.sample_duration_uS;
+
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay;
+}
+
+UnpackerHandler_DenseCapsuleNode::UnpackerHandler_DenseCapsuleNode()
+    : _cached_scan_node_buf_pos(0)
+    , _is_previous_capsuledataRdy(false)
+    , _cached_last_data_timestamp_us(0)
+
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_dense_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_DenseCapsuleNode::~UnpackerHandler_DenseCapsuleNode()
+{
+
+}
+
+void UnpackerHandler_DenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+
+_u8 UnpackerHandler_DenseCapsuleNode::getSampleAnswerType() const
+{
+    return RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED;
+}
+
+
+void UnpackerHandler_DenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit 1
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) {
+                // pass
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the sync bit 2
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_dense_capsule_measurement_nodes_t* node = reinterpret_cast<rplidar_response_dense_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+            // calc the checksum ...
+            _u8 checksum = 0;
+            _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4));
+            for (size_t cpos = offsetof(rplidar_response_dense_capsule_measurement_nodes_t, start_angle_sync_q6);
+                cpos < sizeof(rplidar_response_dense_capsule_measurement_nodes_t); ++cpos)
+            {
+                checksum ^= _cached_scan_node_buf[cpos];
+            }
+
+            if (recvChecksum == checksum)
+            {
+                // only consider vaild if the checksum matches...
+
+                // perform data endianess convertion if necessary
+#ifdef _CPU_ENDIAN_BIG
+                node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6);
+                for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) {
+                    node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1);
+                    node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2);
+                }
+#endif
+                if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT)
+                {
+                    if (_is_previous_capsuledataRdy) {
+                        engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET
+                            , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node));
+                    }
+                    // this is the first capsule frame in logic, discard the previous cached data...
+                    _is_previous_capsuledataRdy = false;
+                    engine->publishNewScanReset();
+
+
+                }
+                _onScanNodeDenseCapsuleData(*node, engine);
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node));
+
+            }
+            continue;
+        }
+        break;
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+}
+
+void UnpackerHandler_DenseCapsuleNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+    _cached_last_data_timestamp_us = 0;
+}
+
+void UnpackerHandler_DenseCapsuleNode::_onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t& dense_capsule, LIDARSampleDataUnpackerInner* engine)
+{
+    static int lastNodeSyncBit = 0;
+    _u64 currentTs = engine->getCurrentTimestamp_uS();
+
+    if (_is_previous_capsuledataRdy) {
+        int diffAngle_q8;
+        int currentStartAngle_q8 = ((dense_capsule.start_angle_sync_q6 & 0x7FFF) << 2);
+        int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
+
+        diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8);
+        if (prevStartAngle_q8 > currentStartAngle_q8) {
+            diffAngle_q8 += (360 << 8);
+        }
+        int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(dense_capsule.cabins) /*40 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8;
+        if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard
+            _cached_previous_dense_capsuledata = dense_capsule;
+            return;
+        }
+
+        int angleInc_q16 = (diffAngle_q8 << 8) / 40;
+        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
+        for (int pos = 0; pos < (int)_countof(_cached_previous_dense_capsuledata.cabins); ++pos)
+        {
+            int dist_q2;
+            int angle_q6;
+            int syncBit;
+            const int dist = static_cast<const int>(_cached_previous_dense_capsuledata.cabins[pos].distance);
+            dist_q2 = dist << 2;
+            angle_q6 = (currentAngle_raw_q16 >> 10);
+            syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0;
+            syncBit = (syncBit ^ lastNodeSyncBit) & syncBit;//Ensure that syncBit is exactly detected
+
+            currentAngle_raw_q16 += angleInc_q16;
+
+            if (angle_q6 < 0) angle_q6 += (360 << 6);
+            if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6);
+
+            rplidar_response_measurement_node_hq_t hqNode;
+
+
+            hqNode.flag = (syncBit | ((!syncBit) << 1));
+            hqNode.quality = dist_q2 ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
+            hqNode.angle_z_q14 = (angle_q6 << 8) / 90;
+            hqNode.dist_mm_q2 = dist_q2;
+            engine->publishHQNode(currentTs - _getSampleDelayOffsetInDenseMode(_cachedTimingDesc, pos), &hqNode);
+            
+            lastNodeSyncBit = syncBit;
+
+        }
+    }
+
+    _cached_previous_dense_capsuledata = dense_capsule;
+    _is_previous_capsuledataRdy = true;
+
+}
+
+// UnpackerHandler_UltraDenseCapsuleNode
+///////////////////////////////////////////////////////////////////////////////////
+
+static _u64 _getSampleDelayOffsetInUltraDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 1000000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+    const _u64 groupingDelay = ((32 * 2 - 1) - sampleIdx) * timing.sample_duration_uS;
+
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay;
+}
+
+
+UnpackerHandler_UltraDenseCapsuleNode::UnpackerHandler_UltraDenseCapsuleNode()
+    : _cached_scan_node_buf_pos(0)
+    , _is_previous_capsuledataRdy(false)
+    , _cached_last_data_timestamp_us(0)
+    , _last_node_sync_bit(0)
+    , _last_dist_q2(0)
+
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_UltraDenseCapsuleNode::~UnpackerHandler_UltraDenseCapsuleNode()
+{
+
+}
+
+
+void UnpackerHandler_UltraDenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+
+_u8 UnpackerHandler_UltraDenseCapsuleNode::getSampleAnswerType() const
+{
+    return RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED;
+}
+
+void UnpackerHandler_UltraDenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit 1
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) {
+                // pass
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the sync bit 2
+        {
+            _u8 tmp = (current_data >> 4);
+            if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                _is_previous_capsuledataRdy = false;
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_ultra_dense_capsule_measurement_nodes_t* node = reinterpret_cast<rplidar_response_ultra_dense_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+            // calc the checksum ...
+            _u8 checksum = 0;
+            _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4));
+            for (size_t cpos = offsetof(rplidar_response_ultra_dense_capsule_measurement_nodes_t, time_stamp);
+                cpos < sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t); ++cpos)
+            {
+                checksum ^= _cached_scan_node_buf[cpos];
+            }
+
+            if (recvChecksum == checksum)
+            {
+                // only consider vaild if the checksum matches...
+
+                // perform data endianess convertion if necessary
+#ifdef _CPU_ENDIAN_BIG
+                node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6);
+                for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) {
+                    node->cabins[cpos].qualityl_distance_scale[0] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[0]);
+                    node->cabins[cpos].qualityl_distance_scale[1] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[1]);
+                }
+#endif
+                if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT)
+                {
+                    if (_is_previous_capsuledataRdy) {
+                        engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET
+                            , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node));
+
+                    }
+                    // this is the first capsule frame in logic, discard the previous cached data...
+                    _is_previous_capsuledataRdy = false;
+                    engine->publishNewScanReset();
+
+                }
+                _onScanNodeUltraDenseCapsuleData(*node, engine);
+            }
+            else {
+                _is_previous_capsuledataRdy = false;
+
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node));
+
+            }
+            continue;
+        }
+        break;
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+void UnpackerHandler_UltraDenseCapsuleNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+    _cached_last_data_timestamp_us = 0;
+    _last_node_sync_bit = 0;
+    _last_dist_q2 = 0;
+}
+
+void UnpackerHandler_UltraDenseCapsuleNode::_onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine)
+{
+    _u64 currentTimestamp = engine->getCurrentTimestamp_uS();
+
+    const rplidar_response_ultra_dense_capsule_measurement_nodes_t* ultra_dense_capsule = reinterpret_cast<const rplidar_response_ultra_dense_capsule_measurement_nodes_t*>(&capsule);
+    if (_is_previous_capsuledataRdy) {
+        int diffAngle_q8;
+        int currentStartAngle_q8 = ((ultra_dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2);
+        int prevStartAngle_q8 = ((_cached_previous_ultra_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
+
+
+
+        diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8);
+        if (prevStartAngle_q8 > currentStartAngle_q8) {
+            diffAngle_q8 += (360 << 8);
+        }
+
+        int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(ultra_dense_capsule->cabins) /*64 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8;
+        if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard
+            _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule;
+            return;
+        }
+#define DISTANCE_THRESHOLD_TO_SCALE_1 2046  // (2^10 - 1)*2 mm
+#define DISTANCE_THRESHOLD_TO_SCALE_2 8187  // (2^11 - 1)*3 + 2046 mm
+#define DISTANCE_THRESHOLD_TO_SCALE_3 24567 // (2^12 - 1)*4 + 8187 mm
+        int angleInc_q16 = (diffAngle_q8 << 8) / 64;
+        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
+        for (int pos = 0; pos < (int)_countof(_cached_previous_ultra_dense_capsuledata.cabins) * 2; ++pos)
+        {
+            int angle_q6;
+            int syncBit;
+            size_t cabin_idx = pos >> 1;
+            _u32  quality_dist_scale;
+            if (!(pos & 0x1)) {
+                quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[0] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array & 0x0F) << 16);
+            }
+            else {
+                quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[1] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array >> 4) << 16);
+            }
+
+            _u8 scale = quality_dist_scale & 0x3;
+            _u8 quality = 0;
+            int dist_q2 = 0;
+
+            switch (scale) {
+            case 0:
+                quality = quality_dist_scale >> 12;
+                dist_q2 = (quality_dist_scale & 0xFFC) * 2;
+                if (_last_dist_q2) {
+                    if (abs(dist_q2 - _last_dist_q2) <= 8/*2mm *2*/) {
+                        dist_q2 = (dist_q2 + _last_dist_q2) >> 1;
+                    }
+                }
+                break;
+            case 1:
+                quality = (quality_dist_scale >> 13) << 1;
+                dist_q2 = (quality_dist_scale & 0x1FFC) * 3 + (DISTANCE_THRESHOLD_TO_SCALE_1 << 2);
+                break;
+            case 2:
+                quality = (quality_dist_scale >> 14) << 2;
+                dist_q2 = (quality_dist_scale & 0x3FFC) * 4 + (DISTANCE_THRESHOLD_TO_SCALE_2 << 2);
+                break;
+            case 3:
+                quality = (quality_dist_scale >> 15) << 3;
+                dist_q2 = (quality_dist_scale & 0x7FFC) * 5 + (DISTANCE_THRESHOLD_TO_SCALE_3 << 2);
+                break;
+            }
+            _last_dist_q2 = dist_q2;
+            angle_q6 = (currentAngle_raw_q16 >> 10);
+            syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0;
+            syncBit = (syncBit ^ _last_node_sync_bit) & syncBit;//Ensure that syncBit is exactly detected
+
+            currentAngle_raw_q16 += angleInc_q16;
+
+            if (angle_q6 < 0) angle_q6 += (360 << 6);
+            if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6);
+
+
+            rplidar_response_measurement_node_hq_t hqNode;
+
+
+
+            hqNode.flag = (syncBit | ((!syncBit) << 1));
+            hqNode.quality = quality;
+            hqNode.angle_z_q14 = (angle_q6 << 8) / 90;
+            hqNode.dist_mm_q2 = dist_q2;
+            engine->publishHQNode(currentTimestamp - _getSampleDelayOffsetInUltraDenseMode(_cachedTimingDesc, pos), &hqNode);
+            
+            _last_node_sync_bit = syncBit;
+
+        }
+    }
+
+    _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule;
+    _is_previous_capsuledataRdy = true;
+
+}
+
+
+
+}
+
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h
new file mode 100644
index 0000000..008f544
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h
@@ -0,0 +1,149 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Capsule Style Sample Node Handlers
+  */
+
+  /*
+	* 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
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_CapsuleNode();
+	virtual ~UnpackerHandler_CapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+
+	void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraCapsuleNode();
+	virtual ~UnpackerHandler_UltraCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+
+class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_DenseCapsuleNode();
+	virtual ~UnpackerHandler_DenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraDenseCapsuleNode();
+	virtual ~UnpackerHandler_UltraDenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+
+
+	int              _last_node_sync_bit;
+	int              _last_dist_q2;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+
+}
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp
new file mode 100644
index 0000000..8ed450e
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp
@@ -0,0 +1,192 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+#include "sl_crc.h" 
+#endif
+
+#include "handler_hqnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_HQNode::UnpackerHandler_HQNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_HQNode::~UnpackerHandler_HQNode()
+{
+
+}
+
+_u8 UnpackerHandler_HQNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ;
+}
+
+void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+
+    for (size_t pos = 0; pos < cnt; ++pos)
+    {
+        _u8 current_data = data[pos];
+
+        switch (_cached_scan_node_buf_pos)
+        {
+        case 0: // expect the sync byte
+        {
+            if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) {
+                // pass
+            }
+            else {
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4:    // get bytes to calculate crc ready
+        {
+           
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+            rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast<rplidar_response_hq_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+            _u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4);
+
+
+#else
+            // calculate crc with boost crc method
+            boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc;
+            std::vector<_u8> crcInputData;
+            crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            //supplement crcInputData to mutiples of 4
+            int leftBytes = 4 - (crcInputData.size() & 3);
+            for (int i = 0; i < leftBytes; i++)
+                crcInputData.push_back(0);
+            mycrc.process_bytes(&crcInputData[0], crcInputData.size());
+            _u32 crcCalc = mycrc.checksum();
+            
+#endif
+
+            _u32 recvCRC = nodesData->crc32;
+#ifdef _CPU_ENDIAN_BIG
+            recvCRC = le32_to_cpu(recvCRC);
+            nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp);
+#endif
+            if (recvCRC == crcCalc)
+            {
+                for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos)
+                {
+                    rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos];
+#ifdef _CPU_ENDIAN_BIG
+                    hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14);
+                    hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2);
+#endif
+                    engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode);
+                }
+            }
+            else  //crc check not passed 
+            {
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData));
+            }
+            continue;
+        }
+        break;
+
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+
+void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_HQNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h
new file mode 100644
index 0000000..178a8c6
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h
@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* 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
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+	class UnpackerHandler_HQNode : public IDataUnpackerHandler {
+	public:
+		UnpackerHandler_HQNode();
+		virtual ~UnpackerHandler_HQNode();
+
+		virtual _u8 getSampleAnswerType() const;
+		virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+		virtual void reset();
+		virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+
+	protected:
+		std::vector<_u8> _cached_scan_node_buf;
+		int              _cached_scan_node_buf_pos;
+		SlamtecLidarTimingDesc _cachedTimingDesc;
+	};
+
+}
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp
new file mode 100644
index 0000000..b799b77
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp
@@ -0,0 +1,159 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+
+#include "handler_normalnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing)
+{
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_NormalNode::UnpackerHandler_NormalNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+;}
+
+UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode()
+{
+
+}
+
+_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT;
+}
+
+void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit and its reverse in this byte
+        {
+            _u8 tmp = (current_data >> 1);
+            if ((tmp ^ current_data) & 0x1) {
+                // pass
+            }
+            else {
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the highest bit to be 1
+        {
+            if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                continue;
+            }
+        }
+        break;
+        case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_measurement_node_t* node = reinterpret_cast<rplidar_response_measurement_node_t*>(&_cached_scan_node_buf[0]);
+#ifdef _CPU_ENDIAN_BIG
+            node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit);
+            node->distance_q2 = le16_to_cpu(node->distance_q2);
+#endif
+            //cast node to rplidar_response_measurement_node_hq_t
+            rplidar_response_measurement_node_hq_t hqNode;
+            hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90;  //transfer to q14 Z-angle
+            hqNode.dist_mm_q2 = node->distance_q2;
+            hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);  // trasfer syncbit to HQ flag field
+            hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;  //remove the last two bits and then make quality from 0-63 to 0-255
+            
+            
+            engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode);
+            continue;
+
+        }
+        break;
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+}
+
+
+void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_NormalNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h
new file mode 100644
index 0000000..9068b1b
--- /dev/null
+++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h
@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* 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
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+class UnpackerHandler_NormalNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_NormalNode();
+	virtual ~UnpackerHandler_NormalNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+}
+
+END_DATAUNPACKER_NS()
\ No newline at end of file
diff --git a/LIDAR/sdk/src/hal/abs_rxtx.h b/LIDAR/sdk/src/hal/abs_rxtx.h
new file mode 100644
index 0000000..54900e3
--- /dev/null
+++ b/LIDAR/sdk/src/hal/abs_rxtx.h
@@ -0,0 +1,88 @@
+/*
+ *  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 "hal/types.h"
+
+namespace rp{ namespace hal{
+
+class serial_rxtx
+{
+public:
+    enum{
+        ANS_OK      = 0,
+        ANS_TIMEOUT = -1,
+        ANS_DEV_ERR = -2,
+    };
+
+    static serial_rxtx * CreateRxTx();
+    static void ReleaseRxTx( serial_rxtx * );
+
+    serial_rxtx():_is_serial_opened(false){}
+    virtual ~serial_rxtx(){}
+
+    virtual void flush( _u32 flags) = 0;
+
+    virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0;
+    virtual bool open() = 0;
+    virtual void close()  = 0;
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual int senddata(const unsigned char * data, size_t size) = 0;
+    virtual int recvdata(unsigned char * data, size_t size) = 0;
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual size_t rxqueue_count() = 0;
+
+    virtual void setDTR() = 0;
+    virtual void clearDTR() = 0;
+    virtual void cancelOperation() {}
+
+    virtual bool isOpened()
+    {
+        return _is_serial_opened;
+    }
+
+protected:
+    volatile bool   _is_serial_opened;
+};
+
+}}
+
+
+
diff --git a/LIDAR/sdk/src/hal/assert.h b/LIDAR/sdk/src/hal/assert.h
new file mode 100644
index 0000000..37cfdd5
--- /dev/null
+++ b/LIDAR/sdk/src/hal/assert.h
@@ -0,0 +1,18 @@
+#ifndef _INFRA_HAL_ASSERT_H
+#define _INFRA_HAL_ASSERT_H
+
+#ifdef WIN32
+#include <crtdbg.h>
+#ifndef assert
+#define assert(x) _ASSERT(x)
+#endif
+#elif defined(_AVR_)
+#define assert(x) 
+#elif defined(__GNUC__)
+#ifndef assert
+#define assert(x) 
+#endif
+#else
+#error assert.h cannot identify your platform
+#endif 
+#endif
diff --git a/LIDAR/sdk/src/hal/byteops.h b/LIDAR/sdk/src/hal/byteops.h
new file mode 100644
index 0000000..c86bde3
--- /dev/null
+++ b/LIDAR/sdk/src/hal/byteops.h
@@ -0,0 +1,94 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Byte Operations
+ *
+ */
+
+#pragma once
+
+// byte swapping operations for compiling time
+
+#define __static_byteswap_16(x)  ((_u16)(				\
+	(((_u16)(x) & (_u16)0x00FFU) << 8) |			\
+	(((_u16)(x) & (_u16)0xFF00U) >> 8)))
+
+#define __static_byteswap_32(x) ((_u32)(				\
+	(((_u32)(x) & (_u32)0x000000FFUL) << 24) |		\
+	(((_u32)(x) & (_u32)0x0000FF00UL) <<  8) |		\
+	(((_u32)(x) & (_u32)0x00FF0000UL) >>  8) |		\
+	(((_u32)(x) & (_u32)0xFF000000UL) >> 24)))
+
+#define __static_byteswap_64(x) ((_u64)(				\
+	(((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) |	\
+	(((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) |	\
+	(((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) |	\
+	(((_u64)(x) & (_u64)0x00000000ff000000ULL) <<  8) |	\
+	(((_u64)(x) & (_u64)0x000000ff00000000ULL) >>  8) |	\
+	(((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) |	\
+	(((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) |	\
+	(((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56)))
+
+
+#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0)
+
+
+static inline _u16 __byteswap_16(_u16 x)
+{
+#ifdef __arch_byteswap_16
+    return __arch_byteswap_16(x);
+#else
+    return __static_byteswap_16(x);
+#endif
+}
+
+static inline _u32 __byteswap_32(_u32 x)
+{
+#ifdef __arch_byteswap_32
+    return __arch_byteswap_32(x);
+#else
+    return __static_byteswap_32(x);
+#endif
+}
+
+static inline _u64 __byteswap_64(_u64 x)
+{
+#ifdef __arch_byteswap_64
+    return __arch_byteswap_64(x);
+#else
+    return __static_byteswap_64(x);
+#endif
+}
+
+
+#ifdef float
+static inline float __byteswap_float(float x)
+{
+#ifdef __arch_byteswap_float
+    return __arch_byteswap_float(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[3]);
+    __fast_swap(raw[1], raw[2]);
+    return x;
+#endif
+}
+#endif
+
+
+#ifdef double
+static inline double __byteswap_double(double x)
+{
+#ifdef __arch_byteswap_double
+    return __arch_byteswap_double(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[7]);
+    __fast_swap(raw[1], raw[6]);
+    __fast_swap(raw[2], raw[5]);
+    __fast_swap(raw[3], raw[4]);
+    return x;
+#endif
+}
+#endif
diff --git a/LIDAR/sdk/src/hal/byteorder.h b/LIDAR/sdk/src/hal/byteorder.h
new file mode 100644
index 0000000..d06a9af
--- /dev/null
+++ b/LIDAR/sdk/src/hal/byteorder.h
@@ -0,0 +1,112 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Endianness Helper
+ *
+ */
+
+#pragma once
+
+
+#if !defined(_CPU_ENDIAN_BIG) && !defined(_CPU_ENDIAN_SMALL)
+// CPU Endianness is not specified, assume little endian.
+#define _CPU_ENDIAN_SMALL
+#endif
+
+#if defined(_CPU_ENDIAN_BIG) && defined(_CPU_ENDIAN_SMALL)
+#error "_CPU_ENDIAN_BIG and _CPU_ENDIAN_SMALL cannot be defined at the same time."
+#endif
+
+#include "hal/byteops.h"
+
+#if defined(_CPU_ENDIAN_SMALL)
+
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) ((_u64)(x))
+#define constant_le64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_le32(x) ((_u32)(x))
+#define constant_le32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_le16(x) ((_u16)(x))
+#define constant_le16_to_cpu(x) ((_u16)(x))
+#define constant_cpu_to_be64(x) (__static_byteswap_64((x)))
+#define constant_be64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_be32(x) (__static_byteswap_32((x)))
+#define constant_be32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_be16(x) (__static_byteswap_16((x)))
+#define constant_be16_to_cpu(x) __static_byteswap_16((_u16)(x))
+
+#define cpu_to_le64(x) ((_u64)(x))
+#define le64_to_cpu(x) ((_u64)(x))
+#define cpu_to_le32(x) ((_u32)(x))
+#define le32_to_cpu(x) ((_u32)(x))
+#define cpu_to_le16(x) ((_u16)(x))
+#define le16_to_cpu(x) ((_u16)(x))
+#define cpu_to_be64(x) (__byteswap_64((x)))
+#define be64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_be32(x) (__byteswap_32((x)))
+#define be32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_be16(x) (__byteswap_16((x)))
+#define be16_to_cpu(x) __byteswap_16((_u16)(x))
+#endif
+
+#define cpu_to_float_le(x) ((float)x)
+#define float_le_to_cpu(x) ((float)x)
+
+#define cpu_to_float_be(x) __byteswap_float(x)
+#define float_be_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_double_le(x) ((double)x)
+#define double_le_to_cpu(x) ((double)x)
+
+#define cpu_to_double_be(x) __byteswap_double(x)
+#define double_be_to_cpu(x) __byteswap_double(x)
+
+#else
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) (__static_byteswap_64((x)))
+#define constant_le64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_le32(x) (__static_byteswap_32((x)))
+#define constant_le32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_le16(x) (__static_byteswap_16((x)))
+#define constant_le16_to_cpu(x) __static_byteswap_16((_u16)(x))
+#define constant_cpu_to_be64(x) ((_u64)(x))
+#define constant_be64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_be32(x) ((_u32)(x))
+#define constant_be32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_be16(x) ((_u16)(x))
+#define constant_be16_to_cpu(x) ((_u16)(x))
+
+#define cpu_to_le64(x) (__byteswap_64((x)))
+#define le64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_le32(x) (__byteswap_32((x)))
+#define le32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_le16(x) (__byteswap_16((x)))
+#define le16_to_cpu(x) __byteswap_16((_u16)(x))
+#define cpu_to_be64(x) ((_u64)(x))
+#define be64_to_cpu(x) ((_u64)(x))
+#define cpu_to_be32(x) ((_u32)(x))
+#define be32_to_cpu(x) ((_u32)(x))
+#define cpu_to_be16(x) ((_u16)(x))
+#define be16_to_cpu(x) ((_u16)(x))
+#endif
+
+
+#define cpu_to_float_le(x) __byteswap_float(x)
+#define float_le_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_float_be(x) ((float)x)
+#define float_be_to_cpu(x) ((float)x)
+
+
+#define cpu_to_double_le(x) __byteswap_double(x)
+#define double_le_to_cpu(x) __byteswap_double(x)
+
+#define cpu_to_double_be(x) ((double)x)
+#define double_be_to_cpu(x) ((double)x)
+
+#endif
diff --git a/LIDAR/sdk/src/hal/event.h b/LIDAR/sdk/src/hal/event.h
new file mode 100644
index 0000000..ada15a5
--- /dev/null
+++ b/LIDAR/sdk/src/hal/event.h
@@ -0,0 +1,206 @@
+/*
+ *  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
+namespace rp{ namespace hal{
+
+class Event
+{
+public:
+    
+    enum
+    {
+        EVENT_OK = 1,
+        EVENT_TIMEOUT = 0xFFFFFFFF,
+        EVENT_FAILED = 0,
+    };
+    
+    Event(bool isAutoReset = true, bool isSignal = false)
+#ifdef _WIN32
+        : _event(NULL)
+#else
+        : _is_signalled(isSignal)
+        , _isAutoReset(isAutoReset)
+#endif
+    {
+#ifdef _WIN32
+        _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 
+#else
+        pthread_mutex_init(&_cond_locker, NULL);
+        pthread_condattr_init(&_cond_attr);
+#ifdef _MACOS
+        // sadly, there is no monotonic clock support for pthread cond variable on MACOS
+        // if time slew is a big issue, try to reimplement it using kqueue/kevent
+#else
+        pthread_condattr_setclock(&_cond_attr, CLOCK_MONOTONIC);
+#endif
+        pthread_cond_init(&_cond_var, &_cond_attr);
+       
+#endif
+    }
+
+    ~ Event()
+    {
+        release();
+    }
+
+    void set( bool isSignal = true )
+    {
+        if (isSignal){
+#ifdef _WIN32
+            SetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+               
+            if ( _is_signalled == false )
+            {
+                _is_signalled = true;
+                pthread_cond_signal(&_cond_var);
+            }
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+        else
+        {
+#ifdef _WIN32
+            ResetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+            _is_signalled = false;
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+    }
+    
+    unsigned long wait( unsigned long timeout = 0xFFFFFFFF )
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_FAILED:
+            return EVENT_FAILED;
+        case WAIT_OBJECT_0:
+            return EVENT_OK;
+        case WAIT_TIMEOUT:
+            return EVENT_TIMEOUT;
+        }
+        return EVENT_OK;
+#else
+        unsigned long ans = EVENT_OK;
+        pthread_mutex_lock( &_cond_locker );
+
+        if ( !_is_signalled )
+        {
+            
+                if (timeout == 0xFFFFFFFF){
+                    pthread_cond_wait(&_cond_var,&_cond_locker);
+                }else
+                {
+                    int timewaitresult = 0;
+#ifdef _MACOS
+                    timespec wait_time;
+                    
+                    wait_time.tv_sec = timeout / 1000;
+                    wait_time.tv_nsec = (timeout%1000)*1000000ULL;
+                
+                    timewaitresult = pthread_cond_timedwait_relative_np(&_cond_var,&_cond_locker,&wait_time);
+#else
+                    timespec wait_time;
+                    clock_gettime(CLOCK_MONOTONIC, &wait_time);
+
+                    wait_time.tv_sec += timeout / 1000;
+                    wait_time.tv_nsec += (timeout%1000)*1000000ULL;
+                
+                    if (wait_time.tv_nsec >= 1000000000)
+                    {
+                       ++wait_time.tv_sec;
+                       wait_time.tv_nsec -= 1000000000;
+                    }
+                    timewaitresult = pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time);
+#endif
+
+                    switch (timewaitresult)
+                    {
+                    case 0:
+                        // signalled
+                        break;
+                    case ETIMEDOUT:
+                        // time up
+                        ans = EVENT_TIMEOUT;
+                        goto _final;
+                        break;
+                    default:
+                        ans = EVENT_FAILED;
+                        goto _final;
+                    }
+       
+            }
+        }
+          
+        //assert(_is_signalled);
+
+        if ( _isAutoReset )
+        {
+            _is_signalled = false;
+        }
+_final:
+        pthread_mutex_unlock( &_cond_locker );
+
+        return ans;
+#endif
+        
+    }
+protected:
+
+    void release()
+    {
+#ifdef _WIN32
+        CloseHandle(_event);
+#else
+        pthread_mutex_destroy(&_cond_locker);
+        pthread_cond_destroy(&_cond_var);
+#endif
+    }
+
+#ifdef _WIN32
+        HANDLE _event;
+#else
+        pthread_cond_t         _cond_var;
+        pthread_mutex_t        _cond_locker;
+        pthread_condattr_t     _cond_attr;
+        bool                   _is_signalled;
+        bool                   _isAutoReset;
+#endif
+};
+}}
diff --git a/LIDAR/sdk/src/hal/locker.h b/LIDAR/sdk/src/hal/locker.h
new file mode 100644
index 0000000..2a18e6b
--- /dev/null
+++ b/LIDAR/sdk/src/hal/locker.h
@@ -0,0 +1,205 @@
+/*
+ *  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
+namespace rp{ namespace hal{ 
+
+class Locker
+{
+public:
+    enum LOCK_STATUS
+    {
+        LOCK_OK = 1,
+        LOCK_TIMEOUT = -1,
+        LOCK_FAILED = 0
+    };
+
+    Locker(bool recusive = false){
+#ifdef _WIN32
+        _lock = NULL;
+#endif
+        init(recusive);
+    }
+
+    ~Locker()
+    {
+        release();
+    }
+
+    Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF)
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_ABANDONED:
+            return LOCK_FAILED;
+        case WAIT_OBJECT_0:
+            return LOCK_OK;
+        case WAIT_TIMEOUT:
+            return LOCK_TIMEOUT;
+        }
+
+#else
+#ifdef _MACOS
+        if (timeout !=0 ) {
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#else
+        if (timeout == 0xFFFFFFFF){
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#endif
+        else if (timeout == 0)
+        {
+            if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK;
+        }
+#ifndef _MACOS
+        else
+        {
+            timespec wait_time;
+            timeval now;
+            gettimeofday(&now,NULL);
+
+            wait_time.tv_sec = timeout/1000 + now.tv_sec;
+            wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000;
+        
+            if (wait_time.tv_nsec >= 1000000000)
+            {
+               ++wait_time.tv_sec;
+               wait_time.tv_nsec -= 1000000000;
+            }
+            switch (pthread_mutex_timedlock(&_lock,&wait_time))
+            {
+            case 0:
+                return LOCK_OK;
+            case ETIMEDOUT:
+                return LOCK_TIMEOUT;
+            }
+        }
+#endif
+#endif
+
+        return LOCK_FAILED;
+    }
+
+
+    void unlock()
+    {
+#ifdef _WIN32
+        if (_recusive) {
+            ReleaseMutex(_lock);
+        } else {
+            ReleaseSemaphore(_lock, 1, NULL);
+        }
+#else
+        pthread_mutex_unlock(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE getLockHandle()
+    {
+        return _lock;
+    }
+#else
+    pthread_mutex_t *getLockHandle()
+    {
+        return &_lock;
+    }
+#endif
+
+
+
+protected:
+    void    init(bool recusive)
+    {
+        
+#ifdef _WIN32
+        if (_recusive = recusive) {
+            _lock = CreateMutex(NULL, FALSE, NULL);
+        } else {
+            _lock = CreateSemaphore(NULL, 1, 1, NULL);
+        }
+#else
+        
+        if (recusive) {
+            pthread_mutexattr_t attr;
+            pthread_mutexattr_init(&attr);
+            pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+            pthread_mutex_init(&_lock, &attr);
+        } else {
+            pthread_mutex_init(&_lock, NULL);
+        }
+#endif
+    }
+
+    void    release()
+    {
+        unlock(); //force unlock before release
+#ifdef _WIN32
+
+        if (_lock) CloseHandle(_lock);
+        _lock = NULL;
+#else
+        pthread_mutex_destroy(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE  _lock;
+    bool _recusive;
+#else
+    pthread_mutex_t _lock;
+#endif
+    
+};
+
+class AutoLocker
+{
+public :
+    AutoLocker(Locker &l): _binded(l)
+    {
+        _binded.lock();
+    }
+
+    void forceUnlock() {
+        _binded.unlock();
+    }
+    ~AutoLocker() {_binded.unlock();}
+    Locker & _binded;
+};
+
+
+}}
+
diff --git a/LIDAR/sdk/src/hal/socket.h b/LIDAR/sdk/src/hal/socket.h
new file mode 100644
index 0000000..cf4fab0
--- /dev/null
+++ b/LIDAR/sdk/src/hal/socket.h
@@ -0,0 +1,149 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace rp{ namespace net {
+
+class _single_thread SocketAddress 
+{
+
+public:
+    enum address_type_t {
+        ADDRESS_TYPE_UNSPEC = 0,
+        ADDRESS_TYPE_INET   = 1,
+        ADDRESS_TYPE_INET6  = 2,
+    };
+
+public:
+
+
+
+    SocketAddress();
+    SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET);
+    // do not use this function, internal usage
+    SocketAddress(void * platform_data);
+    SocketAddress(const SocketAddress &);
+    
+    SocketAddress & operator = (const SocketAddress &);
+
+    virtual ~SocketAddress();
+    
+    virtual int getPort() const;
+    virtual u_result setPort(int port);
+
+    virtual u_result setAddressFromString(const char * address_string,  address_type_t = ADDRESS_TYPE_INET);
+    virtual u_result getAddressAsString(char * buffer, size_t buffersize) const;
+    
+    virtual address_type_t getAddressType() const; 
+
+    virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const;
+
+    const void * getPlatformData() const {
+        return _platform_data;
+    }
+
+    virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET);
+    virtual void setBroadcastAddressIPv4();
+    virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET);
+
+public:
+    static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET);
+
+protected:
+    void * _platform_data;
+};
+
+
+
+class SocketBase 
+{
+public:
+    enum socket_family_t {
+        SOCKET_FAMILY_INET  = 0,
+        SOCKET_FAMILY_INET6 = 1,
+        SOCKET_FAMILY_RAW   = 2,
+    };
+
+
+    enum socket_direction_mask {
+        SOCKET_DIR_RD = 0x1,
+        SOCKET_DIR_WR = 0x2,
+        SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR),
+    };
+
+    enum {
+        DEFAULT_SOCKET_TIMEOUT = 10000, //10sec
+    };
+
+    virtual ~SocketBase() {}
+    virtual void dispose() = 0;
+    virtual u_result bind(const SocketAddress & ) = 0;
+    
+    virtual u_result getLocalAddress(SocketAddress & ) = 0;
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0;
+
+    virtual u_result waitforSent(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+    virtual u_result waitforData(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT)  = 0;
+protected:
+    SocketBase() {} 
+};
+
+
+class _single_thread StreamSocket : public SocketBase
+{
+public:
+
+    enum {
+        MAX_BACKLOG = 128,
+    };
+
+    static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+    
+    virtual u_result connect(const SocketAddress & pairAddress) = 0;
+    
+    virtual u_result listen(int backlog = MAX_BACKLOG) = 0;
+    virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0;
+    virtual u_result waitforIncomingConnection(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+
+    virtual u_result send(const void * buffer, size_t len) = 0;
+    
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0;
+    
+    virtual u_result getPeerAddress(SocketAddress & ) = 0;
+
+    virtual u_result shutdown(socket_direction_mask mask) = 0;
+
+    virtual u_result enableKeepAlive(bool enable = true) = 0;
+    
+    virtual u_result enableNoDelay(bool enable = true) = 0;
+
+protected:
+    virtual ~StreamSocket() {} // use dispose();
+    StreamSocket() {}
+};
+
+class _single_thread DGramSocket: public SocketBase
+{
+
+public:
+
+    static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+       
+    virtual u_result setPairAddress(const SocketAddress * pairAddress) = 0;
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) = 0;
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0;
+    virtual u_result clearRxCache() = 0;
+    
+protected:
+    virtual ~DGramSocket() {} // use dispose();
+
+    DGramSocket() {}
+};
+
+}}
diff --git a/LIDAR/sdk/src/hal/thread.cpp b/LIDAR/sdk/src/hal/thread.cpp
new file mode 100644
index 0000000..bc68dd5
--- /dev/null
+++ b/LIDAR/sdk/src/hal/thread.cpp
@@ -0,0 +1,48 @@
+/*
+ *  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.
+ *
+ */
+
+#include "sdkcommon.h"
+#include "hal/thread.h"
+
+#if defined(_WIN32)
+#include "arch/win32/winthread.hpp"
+#elif defined(_MACOS)
+#include "arch/macOS/thread.hpp"
+#elif defined(__GNUC__)
+#include "arch/linux/thread.hpp"
+#else
+#error no threading implemention found for this platform.
+#endif
+
+
diff --git a/LIDAR/sdk/src/hal/thread.h b/LIDAR/sdk/src/hal/thread.h
new file mode 100644
index 0000000..74b6ff7
--- /dev/null
+++ b/LIDAR/sdk/src/hal/thread.h
@@ -0,0 +1,94 @@
+/*
+ *  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 "hal/types.h"
+#define CLASS_THREAD(c , x ) \
+	rp::hal::Thread::create_member<c, &c::x>(this )
+
+namespace rp{ namespace hal{
+
+class Thread
+{
+public:
+    enum priority_val_t
+	{
+		PRIORITY_REALTIME = 0,
+		PRIORITY_HIGH     = 1,
+		PRIORITY_NORMAL   = 2,
+		PRIORITY_LOW      = 3,
+		PRIORITY_IDLE     = 4,
+	};
+
+    template <class T, u_result (T::*PROC)(void)>
+    static Thread create_member(T * pthis)
+    {
+		return create(_thread_thunk<T,PROC>, pthis);
+	}
+
+	template <class T, u_result (T::*PROC)(void) >
+	static _word_size_t THREAD_PROC _thread_thunk(void * data)
+	{
+		return (static_cast<T *>(data)->*PROC)();
+	}
+	static Thread create(thread_proc_t proc, void * data = NULL );
+
+public:
+    ~Thread() { }
+    Thread():  _data(NULL),_func(NULL),_handle(0)  {}
+    _word_size_t getHandle(){ return _handle;}
+    u_result terminate();
+    void *getData() { return _data;}
+    u_result join(unsigned long timeout = -1);
+	
+    // disabled as on platforms like Linux, the priority will be inherited by the child thread
+    // which may caused unexpected behavior.
+    // Please using Thread::SetSelfPriority instead
+    // u_result setPriority( priority_val_t p);
+	priority_val_t getPriority();
+
+    static u_result SetSelfPriority(priority_val_t p);
+
+
+    bool operator== ( const Thread & right) { return this->_handle == right._handle; }
+protected:
+    Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0)  {}
+    void * _data;
+    thread_proc_t _func;
+    _word_size_t _handle;
+};
+
+}}
+
diff --git a/LIDAR/sdk/src/hal/types.h b/LIDAR/sdk/src/hal/types.h
new file mode 100644
index 0000000..6e23faa
--- /dev/null
+++ b/LIDAR/sdk/src/hal/types.h
@@ -0,0 +1,119 @@
+/*
+ *      Common Data Types for RP
+ */
+
+#ifndef _INFRA_HAL_TYPES_H_
+#define _INFRA_HAL_TYPES_H_
+
+//Basic types
+//
+#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;
+
+
+#define RPMODULE_EXPORT  __declspec(dllexport)
+#define RPMODULE_IMPORT  __declspec(dllimport)
+
+#else
+
+#include <stdint.h>
+
+#define RPMODULE_EXPORT
+#define RPMODULE_IMPORT
+
+#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 size_t         _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
+
+
+
+//#define __le 
+//#define __be
+
+#define _multi_thread
+#define _single_thread
+
+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 RESULT_OPERATION_ABORTED        (0x8007 | RESULT_FAIL_BIT)
+#define RESULT_NOT_FOUND                (0x8008 | RESULT_FAIL_BIT)
+#define RESULT_RECONNECTING             (0x8009 | 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 * );
+
+
+#if defined (_BUILD_AS_DLL)
+#if defined (_BUILD_DLL_EXPORT)
+#define RPMODULE_IMPEXP  RPMODULE_EXPORT
+#else
+#define RPMODULE_IMPEXP  RPMODULE_IMPORT
+#endif
+#else
+#define RPMODULE_IMPEXP 
+#endif
+
+#endif
diff --git a/LIDAR/sdk/src/hal/util.h b/LIDAR/sdk/src/hal/util.h
new file mode 100644
index 0000000..205a858
--- /dev/null
+++ b/LIDAR/sdk/src/hal/util.h
@@ -0,0 +1,67 @@
+/*
+ *  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
+
+
+//------
+/* _countof helper */
+#if !defined(_countof)
+#if !defined(__cplusplus)
+#define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0]))
+#else
+extern "C++"
+{
+template <typename _CountofType, size_t _SizeOfArray>
+char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray];
+#define _countof(_Array) sizeof(*__countof_helper(_Array))
+}
+#endif
+#endif
+
+/* _offsetof helper */
+#if !defined(offsetof)
+#define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field))
+#endif
+
+
+#define BEGIN_STATIC_CODE( _blockname_ ) \
+    static class _static_code_##_blockname_ {   \
+    public:     \
+        _static_code_##_blockname_ () 
+
+
+#define END_STATIC_CODE( _blockname_ ) \
+    }   _instance_##_blockname_;
+
diff --git a/LIDAR/sdk/src/hal/waiter.h b/LIDAR/sdk/src/hal/waiter.h
new file mode 100644
index 0000000..7f83b9b
--- /dev/null
+++ b/LIDAR/sdk/src/hal/waiter.h
@@ -0,0 +1,44 @@
+/*
+ * For synchronize asynchrous operations
+ *
+ * Copyright 2010 Robopeak Team
+ */
+#pragma once
+
+#ifdef _AVR_
+#error there is no implementation for waiter.h on AVR platforms
+#else
+
+#include "hal/event.h"
+
+namespace rp{ namespace hal{
+
+	template<typename ResultT>
+	class Waiter : public Event
+	{
+	public:
+		Waiter() 
+			: Event()
+		{
+		}
+
+		~Waiter() 
+		{}
+
+		ResultT waitForResult()
+		{
+			wait();
+			return result;
+		}
+
+		void setResult(ResultT result)
+		{
+			this->result = result;
+			set();
+		}
+
+		volatile ResultT result;
+	};
+}}
+
+#endif
diff --git a/LIDAR/sdk/src/rplidar_driver.cpp b/LIDAR/sdk/src/rplidar_driver.cpp
new file mode 100644
index 0000000..beedd39
--- /dev/null
+++ b/LIDAR/sdk/src/rplidar_driver.cpp
@@ -0,0 +1,199 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "rplidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+namespace rp { namespace standalone{ namespace rplidar {
+
+    RPlidarDriver::RPlidarDriver(){}
+
+    RPlidarDriver::RPlidarDriver(sl_u32 channelType) 
+        :_channelType(channelType)
+    {
+    }
+
+    RPlidarDriver::~RPlidarDriver() {}
+
+    RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype)
+    {
+        //_channelType = drivertype;
+        return  new RPlidarDriver(drivertype);
+    }
+
+    void RPlidarDriver::DisposeDriver(RPlidarDriver * drv)
+    {
+        delete drv;
+    }
+
+    u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag)
+    {
+        switch (_channelType)
+        {
+        case CHANNEL_TYPE_SERIALPORT:
+            _channel = (*createSerialPortChannel(path, portOrBaud));
+            break;
+        case CHANNEL_TYPE_TCP:
+            _channel = *createTcpChannel(path, portOrBaud);
+            break;
+        case CHANNEL_TYPE_UDP:
+            _channel = *createUdpChannel(path, portOrBaud);
+            break;
+        }
+        if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL;
+        
+        _lidarDrv = *createLidarDriver();
+
+        if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL;
+
+        sl_result ans =(_lidarDrv)->connect(_channel);
+        return ans;
+    }
+
+    void RPlidarDriver::disconnect()
+    {
+        (_lidarDrv)->disconnect();
+    }
+
+    bool RPlidarDriver::isConnected() 
+    { 
+        return (_lidarDrv)->isConnected();
+    }
+     
+    u_result RPlidarDriver::reset(_u32 timeout)
+    {
+        return (_lidarDrv)->reset();
+    }
+
+    u_result RPlidarDriver::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode)
+    {
+        return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode);
+    }
+
+    u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout)
+    {
+        return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout);
+    }
+    
+    u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout)
+    {
+        return (_lidarDrv)->getHealth(health, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout)
+    {
+        return (_lidarDrv)->getDeviceInfo(info, timeout);
+    }
+
+    u_result RPlidarDriver::setMotorPWM(_u16 pwm)
+    {
+        return (_lidarDrv)->setMotorSpeed(pwm);
+    }   
+    
+    u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout)
+    {
+        MotorCtrlSupport motorSupport;
+        u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout);
+        if (motorSupport == MotorCtrlSupportNone)
+            support = false;
+        return ans;
+    }
+
+    u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout)
+	{
+		return (_lidarDrv)->setLidarIpConf(conf, timeout);
+	}
+
+    u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout)
+    {
+        return (_lidarDrv)->getLidarIpConf(conf, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs)
+	{
+		return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs);
+	}
+
+    u_result RPlidarDriver::stop(_u32 timeout) 
+    { 
+        return (_lidarDrv)->stop(timeout);
+    }
+
+    u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout)
+    {
+        return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout);
+    }
+
+    u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count)
+    {
+        return (_lidarDrv)->ascendScanData(nodebuffer, count);
+    }
+    
+    u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)
+    {
+        return RESULT_OPERATION_NOT_SUPPORT;
+    }
+
+    u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count)
+    {
+        return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count);
+    }
+
+    u_result RPlidarDriver::startMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(DEFAULT_MOTOR_SPEED);
+    }
+    u_result RPlidarDriver::stopMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(0);
+    }
+
+}}}
diff --git a/LIDAR/sdk/src/sdkcommon.h b/LIDAR/sdk/src/sdkcommon.h
new file mode 100644
index 0000000..f928b42
--- /dev/null
+++ b/LIDAR/sdk/src/sdkcommon.h
@@ -0,0 +1,51 @@
+/*
+ *  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.
+ *
+ */
+
+#if defined(_WIN32)
+
+#include "arch/win32/arch_win32.h"
+#elif defined(_MACOS)
+#include "arch/macOS/arch_macOS.h"
+#elif defined(__GNUC__)
+#include "arch/linux/arch_linux.h"
+#else
+#error "unsupported target"
+#endif
+
+#include "hal/types.h"
+#include "hal/assert.h"
+
+#include "rplidar.h"
+
+#include "hal/util.h"
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_async_transceiver.cpp b/LIDAR/sdk/src/sl_async_transceiver.cpp
new file mode 100644
index 0000000..c8033a5
--- /dev/null
+++ b/LIDAR/sdk/src/sl_async_transceiver.cpp
@@ -0,0 +1,412 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_async_transceiver.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+
+ProtocolMessage::ProtocolMessage()
+        : len(0)
+        , cmd(0)
+        , data(NULL)
+        , _databufsize(0)
+		, _usingOutterData(false)
+{
+    _changeBufSize();
+}
+
+ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size)
+	: len(size)
+	, cmd(cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize();
+	if (buffer)
+	{
+		memcpy(data, buffer, size);
+	}
+}
+
+ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg)
+	: len(srcMsg.len)
+	, cmd(srcMsg.cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize( true );
+	if (srcMsg.data && len)
+	{
+		memcpy(data, srcMsg.data, len);
+	}
+}
+
+ProtocolMessage::~ProtocolMessage()
+{
+	this->cleanData();
+}
+
+ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage)
+{
+	this->cleanData();
+
+
+	this->len = srcMessage.len;
+	this->cmd = srcMessage.cmd;
+
+    _changeBufSize( true );
+	if (srcMessage.data && len)
+	{
+		memcpy(data, srcMessage.data, len);
+	}
+
+	return *this;
+}
+
+void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size)
+{
+	this->cleanData();
+
+	len = size;
+	data = buffer;
+    _databufsize = size;
+	_usingOutterData = true;
+}
+
+void ProtocolMessage::fillData(const void * buffer, size_t size)
+{
+	len = size;
+    _changeBufSize();
+    if (buffer)
+	    memcpy(data, buffer, size);
+}
+
+void ProtocolMessage::cleanData()
+{
+	if (data) 
+	{
+		if (!_usingOutterData)
+		{
+			delete [] data;
+		}
+		data = NULL;
+		len = 1;
+        _databufsize = 0;
+	}
+}
+
+void ProtocolMessage::_changeBufSize( bool force_compact)
+{
+    size_t actual_size  = getPayloadSize();
+
+    size_t new_buf_size = actual_size;
+
+
+    if (!_usingOutterData)
+    {
+        // nothing to do
+        if ( new_buf_size == _databufsize ) return;
+
+        if ( new_buf_size < _databufsize){
+
+            if ( (_databufsize >> 1) < new_buf_size)
+            {
+                // reuse the current buffer
+                if (!force_compact) return;
+            }else
+            {
+                // the current buffer size is much bigger, we need to release it to save memory
+            }
+        }
+    }
+
+    // we need to change the buffer
+    cleanData();
+    // the cleanData() will reset the length info, so we need to restore it
+    len = actual_size;
+    data = new _u8[new_buf_size];
+    _databufsize = new_buf_size;
+}
+
+
+AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec)
+	: _bindedChannel(NULL)
+	, _codec(codec)
+	, _isWorking(false)
+    , _workingFlag(0)
+{
+
+}
+
+AsyncTransceiver::~AsyncTransceiver()
+{
+    unbindAndClose();
+}
+
+u_result AsyncTransceiver::openChannelAndBind(IChannel* channel)
+{
+    if (!channel) return RESULT_INVALID_DATA;
+
+    unbindAndClose();
+	u_result ans = RESULT_OK;
+	do 
+	{
+		rp::hal::AutoLocker l(_opLocker);
+
+        // try to open the channel ...
+        Result<nullptr_t> ans = SL_RESULT_OK;
+
+        if (!channel->open()) {
+            ans= RESULT_OPERATION_FAIL;
+            break;
+        }
+
+
+        // force a flush to clear any pending data
+        channel->flush();
+
+		_dataEvt.set(false);
+
+		_isWorking = true;
+        _workingFlag = 0;
+        _bindedChannel = channel;
+
+
+		_decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread);
+		_rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread);
+
+        
+
+
+	} while (0);
+
+	return ans;
+}
+
+void AsyncTransceiver::unbindAndClose()
+{
+	rp::hal::AutoLocker l(_opLocker);
+	if (!_isWorking) return;
+
+    assert(_bindedChannel);
+
+    
+	_isWorking = false;
+	_dataEvt.set(); // set signal to wake up threads
+
+	_decoderThread.join();
+	_rxThread.join();
+
+
+    _bindedChannel->close();
+
+    _bindedChannel = NULL;
+
+
+    for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr)
+    {
+        delete [] *itr;
+    }
+    _rxQueue.clear();
+
+}
+
+u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg)
+{
+    assert(msg);
+
+    if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT;
+
+    rp::hal::AutoLocker l(_opLocker);
+
+    size_t requiredBufferSize = _codec.estimateLength(msg);
+
+    if (requiredBufferSize == 0) {
+        // nothing to send
+        return RESULT_OK;
+    }
+
+    u_result ans = RESULT_OK;
+
+    _u8* txBuffer = new _u8[requiredBufferSize];
+
+    do {
+  
+        if (!txBuffer) {
+            return RESULT_INSUFFICIENT_MEMORY;
+        }
+
+        _codec.onEncodeData(msg, txBuffer, &requiredBufferSize);
+
+        int txSize = _bindedChannel->write(txBuffer, requiredBufferSize);
+
+        if (txSize < 0) ans = RESULT_OPERATION_FAIL;
+
+    } while (0);
+
+
+    delete[] txBuffer;
+    return ans;
+}
+
+sl_result AsyncTransceiver::_proc_rxThread()
+{
+    assert(_bindedChannel);
+
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+
+    u_result result;
+    size_t hintedSize = 0;
+    while (_isWorking)
+    {
+        result = _bindedChannel->waitForDataExt(hintedSize, 1000);
+
+        if (IS_FAIL(result))
+        {
+            // timeout is allowed
+            if (result == RESULT_OPERATION_TIMEOUT) {
+                continue;
+            }
+            if (_isWorking) {
+                _workingFlag |= WORKING_FLAG_ERROR;
+                _codec.onChannelError(result);
+                break;
+            }
+        }
+
+        // no data in buffer, sleep and wait for the next round
+        if (!hintedSize)
+        {
+            continue;
+        }
+
+
+        Buffer* decodeBuffer = new Buffer();
+        
+        decodeBuffer->data = new _u8[hintedSize];
+
+        decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize);
+#ifdef _DEBUG_DUMP_PACKET
+        printf("Revc: %d\n", decodeBuffer->size);
+#endif
+         
+        if  (!decodeBuffer->size) {
+            delete decodeBuffer;
+
+            
+            _workingFlag |= WORKING_FLAG_ERROR;
+            _codec.onChannelError(RESULT_OPERATION_ABORTED);
+            break;
+        }
+
+        assert(hintedSize >= decodeBuffer->size);
+
+
+#ifdef _DEBUG_DUMP_PACKET
+        printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size);
+        for (int pos = 0; pos < decodeBuffer->size; pos++)
+        {
+            printf("%02x ", decodeBuffer->data[pos]);
+        }
+        printf("\n=== END ===\n");
+#endif
+
+        _rxLocker.lock();
+        _rxQueue.push_back(decodeBuffer);
+        _dataEvt.set();
+        _rxLocker.unlock();
+
+
+    }
+    _workingFlag |= WORKING_FLAG_RX_DISABLED;
+    return RESULT_OK;
+}
+
+sl_result AsyncTransceiver::_proc_decoderThread()
+{
+
+    assert(_bindedChannel);
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+    _codec.onDecodeReset();
+    
+
+    while (_isWorking)
+    {
+        _rxLocker.lock();
+
+        if (_rxQueue.empty())
+        {
+            _rxLocker.unlock();
+
+            if (_dataEvt.wait(1000))
+                continue;
+
+            _rxLocker.lock();
+        }
+        assert(!_rxQueue.empty());
+
+        Buffer * bufferToDecode = _rxQueue.front();
+        _rxQueue.pop_front();
+
+        _rxLocker.unlock();
+
+        //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<<endl;
+        _codec.onDecodeData(bufferToDecode->data, bufferToDecode->size);
+
+
+        delete bufferToDecode;
+    }
+
+    return RESULT_OK;
+
+}
+
+
+}}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_async_transceiver.h b/LIDAR/sdk/src/sl_async_transceiver.h
new file mode 100644
index 0000000..75774e8
--- /dev/null
+++ b/LIDAR/sdk/src/sl_async_transceiver.h
@@ -0,0 +1,167 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 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 <list>
+#include <memory>
+
+namespace sl { namespace internal {
+
+
+class _single_thread ProtocolMessage {
+
+public:
+	size_t len;			
+	_u8 cmd;
+protected:
+	_u8* data;
+	size_t _databufsize;
+
+public:
+	ProtocolMessage();
+	ProtocolMessage(_u8 cmd, const void* buffer, size_t size);
+	ProtocolMessage(const ProtocolMessage& srcMsg);
+	virtual ~ProtocolMessage();
+
+	ProtocolMessage& operator=(const ProtocolMessage& srcMessage);
+
+	// avoid use this method, pls. use fillData instead
+	void setDataBuf(_u8* buffer, size_t size);
+
+	_u8* getDataBuf() { return data; }
+
+	void fillData(const void* buffer, size_t size);
+	void cleanData();
+
+	size_t getPayloadSize() const
+	{
+		return len;
+	}
+
+protected:
+
+	// change the data buffer to fix the new payload size
+	// the existing buffer will be reused if possible.
+	// all the existing payload data will lose
+	void _changeBufSize(bool force_compact = false);
+	bool _usingOutterData;
+};
+
+
+
+typedef std::shared_ptr<ProtocolMessage> message_autoptr_t;
+
+
+class IAsyncProtocolCodec {
+public:
+	IAsyncProtocolCodec() {}
+	virtual ~IAsyncProtocolCodec()  {}
+
+	virtual void   onChannelError(u_result errCode) {}
+
+	virtual void   onDecodeReset() {}
+	virtual void   onDecodeData(const void* buffer, size_t size) = 0;
+
+
+	virtual size_t estimateLength(message_autoptr_t& message) = 0;
+	virtual void   onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size) = 0;
+
+};
+
+class AsyncTransceiver {
+public:
+
+	enum working_flag_t
+	{
+		WORKING_FLAG_RX_DISABLED = 0x1L << 0,
+		WORKING_FLAG_TX_DISABLED = 0x1L << 1,
+
+		WORKING_FLAG_ERROR = 0x1L << 31,
+	};
+
+
+	AsyncTransceiver(IAsyncProtocolCodec& codec);
+	~AsyncTransceiver();
+
+
+
+	u_result openChannelAndBind(IChannel* channel);
+	void     unbindAndClose();
+
+	IChannel* getBindedChannel() const {
+		return _bindedChannel;
+	}
+	
+	u_result sendMessage(message_autoptr_t& msg);
+
+protected:
+
+	sl_result _proc_rxThread();
+	sl_result _proc_decoderThread();
+
+protected:
+
+
+	rp::hal::Locker _opLocker;
+	rp::hal::Locker _rxLocker;
+	rp::hal::Event  _dataEvt;
+
+	IChannel* _bindedChannel;
+	IAsyncProtocolCodec& _codec;
+
+
+	bool _isWorking;
+	_u32 _workingFlag;
+
+	rp::hal::Thread _rxThread;
+	rp::hal::Thread _decoderThread;
+
+	struct Buffer {
+		size_t size;
+		_u8* data;
+
+
+		Buffer() : size(0), data(NULL){}
+
+		~Buffer() {
+			if (data) {
+				delete[] data;
+				data = NULL;
+			}
+		}
+	};
+	std::list< Buffer * > _rxQueue;
+};
+
+
+}}
diff --git a/LIDAR/sdk/src/sl_crc.cpp b/LIDAR/sdk/src/sl_crc.cpp
new file mode 100644
index 0000000..49d7844
--- /dev/null
+++ b/LIDAR/sdk/src/sl_crc.cpp
@@ -0,0 +1,102 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_crc.h"  
+
+namespace sl {namespace crc32 {
+    
+    static sl_u32 table[256];//crc32_table
+    sl_u32 bitrev(sl_u32 input, sl_u16 bw)
+    {
+        sl_u16 i;
+        sl_u32 var;
+        var = 0;
+        for (i = 0; i < bw; i++) {
+            if (input & 0x01) {
+                var |= 1 << (bw - 1 - i);
+            }
+            input >>= 1;
+        }
+        return var;
+    }
+
+    void init(sl_u32 poly)
+    {
+        sl_u16 i;
+        sl_u16 j;
+        sl_u32 c;
+
+        poly = bitrev(poly, 32);
+        for (i = 0; i < 256; i++) {
+            c = i;
+            for (j = 0; j < 8; j++) {
+                if (c & 1)
+                    c = poly ^ (c >> 1);
+                else
+                    c = c >> 1;
+            }
+            table[i] = c;
+        }
+    }
+
+    sl_u32 cal(sl_u32 crc, void* input, sl_u16 len)
+    {
+        sl_u16 i;
+        sl_u8 index;
+        sl_u8* pch;
+        pch = (unsigned char*)input;
+        sl_u8 leftBytes = 4 - (len & 0x3);
+
+        for (i = 0; i < len; i++) {
+            index = (unsigned char)(crc^*pch);
+            crc = (crc >> 8) ^ table[index];
+            pch++;
+        }
+
+        for (i = 0; i < leftBytes; i++) {//zero padding
+            index = (unsigned char)(crc ^ 0);
+            crc = (crc >> 8) ^ table[index];
+        }
+        return crc ^ 0xffffffff;
+    }
+
+    sl_result getResult(sl_u8 *ptr, sl_u32 len) 
+    {
+        static sl_u8 tmp;
+        if (tmp != 1) {
+            init(0x4C11DB7);
+            tmp = 1;
+        }
+
+        return cal(0xFFFFFFFF, ptr, len);
+    }
+}}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_lidar_driver.cpp b/LIDAR/sdk/src/sl_lidar_driver.cpp
new file mode 100644
index 0000000..4fd719c
--- /dev/null
+++ b/LIDAR/sdk/src/sl_lidar_driver.cpp
@@ -0,0 +1,1702 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "hal/waiter.h"
+#include "hal/byteorder.h"
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+#include <memory>
+#include <atomic>
+#include <deque>
+
+#include "dataunpacker/dataunpacker.h"
+#include "sl_async_transceiver.h"
+#include "sl_lidarprotocol_codec.h"
+
+
+
+#ifdef _WIN32
+#define NOMINMAX
+#undef min
+#undef max
+#endif
+
+#if defined(__cplusplus) && __cplusplus >= 201103L
+#ifndef _GXX_NULLPTR_T
+#define _GXX_NULLPTR_T
+typedef decltype(nullptr) nullptr_t;
+#endif
+#endif /* C++11.  */
+
+namespace sl {
+    static void printDeprecationWarn(const char* fn, const char* replacement)
+    {
+        fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement);
+    }
+
+    static void convert(const sl_lidar_response_measurement_node_t& from, sl_lidar_response_measurement_node_hq_t& to)
+    {
+        to.angle_z_q14 = (((from.angle_q6_checkbit) >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90;  //transfer to q14 Z-angle
+        to.dist_mm_q2 = from.distance_q2;
+        to.flag = (from.sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT);  // trasfer syncbit to HQ flag field
+        to.quality = (from.sync_quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;  //remove the last two bits and then make quality from 0-63 to 0-255
+    }
+
+    static void convert(const sl_lidar_response_measurement_node_hq_t& from, sl_lidar_response_measurement_node_t& to)
+    {
+        to.sync_quality = (from.flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+        to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT);
+        to.distance_q2 = from.dist_mm_q2 > sl_u16(-1) ? sl_u16(0) : sl_u16(from.dist_mm_q2);
+    }
+
+
+    static inline float getAngle(const sl_lidar_response_measurement_node_t& node)
+    {
+        return (node.angle_q6_checkbit >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f;
+    }
+
+    static inline void setAngle(sl_lidar_response_measurement_node_t& node, float v)
+    {
+        sl_u16 checkbit = node.angle_q6_checkbit & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT;
+        node.angle_q6_checkbit = (((sl_u16)(v * 64.0f)) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit;
+    }
+
+    static inline float getAngle(const sl_lidar_response_measurement_node_hq_t& node)
+    {
+        return node.angle_z_q14 * 90.f / 16384.f;
+    }
+
+    static inline void setAngle(sl_lidar_response_measurement_node_hq_t& node, float v)
+    {
+        node.angle_z_q14 = sl_u32(v * 16384.f / 90.f);
+    }
+
+    static inline sl_u16 getDistanceQ2(const sl_lidar_response_measurement_node_t& node)
+    {
+        return node.distance_q2;
+    }
+
+    static inline sl_u32 getDistanceQ2(const sl_lidar_response_measurement_node_hq_t& node)
+    {
+        return node.dist_mm_q2;
+    }
+   
+    template <class TNode>
+    static bool angleLessThan(const TNode& a, const TNode& b)
+    {
+        return getAngle(a) < getAngle(b);
+    }
+
+    template < class TNode >
+    static sl_result ascendScanData_(TNode * nodebuffer, size_t count)
+    {
+        float inc_origin_angle = 360.f / count;
+        size_t i = 0;
+
+        //Tune head
+        for (i = 0; i < count; i++) {
+            if (getDistanceQ2(nodebuffer[i]) == 0) {
+                continue;
+            }
+            else {
+                while (i != 0) {
+                    i--;
+                    float expect_angle = getAngle(nodebuffer[i + 1]) - inc_origin_angle;
+                    if (expect_angle < 0.0f) expect_angle = 0.0f;
+                    setAngle(nodebuffer[i], expect_angle);
+                }
+                break;
+            }
+        }
+
+        // all the data is invalid
+        if (i == count) return SL_RESULT_OPERATION_FAIL;
+
+        //Tune tail
+        for (i = count - 1; i < count; i--) {
+            // To avoid array overruns, use the i < count condition
+            if (getDistanceQ2(nodebuffer[i]) == 0) {
+                continue;
+            }
+            else {
+                while (i != (count - 1)) {
+                    i++;
+                    float expect_angle = getAngle(nodebuffer[i - 1]) + inc_origin_angle;
+                    if (expect_angle > 360.0f) expect_angle -= 360.0f;
+                    setAngle(nodebuffer[i], expect_angle);
+                }
+                break;
+            }
+        }
+
+        //Fill invalid angle in the scan
+        float frontAngle = getAngle(nodebuffer[0]);
+        for (i = 1; i < count; i++) {
+            if (getDistanceQ2(nodebuffer[i]) == 0) {
+                float expect_angle = frontAngle + i * inc_origin_angle;
+                if (expect_angle > 360.0f) expect_angle -= 360.0f;
+                setAngle(nodebuffer[i], expect_angle);
+            }
+        }
+
+        // Reorder the scan according to the angle value
+        std::sort(nodebuffer, nodebuffer + count, &angleLessThan<TNode>);
+
+        return SL_RESULT_OK;
+    }
+
+    template<typename T>
+    class RawSampleNodeHolder
+    {
+    public:
+        RawSampleNodeHolder(size_t maxcount = 8192)
+            : _max_count(maxcount)
+        {
+           
+        }
+        void clear()
+        {
+            rp::hal::AutoLocker l(_locker);
+            _data_waiter.set(false);
+            _data_queue.clear();
+        }
+
+        void pushNode(_u64 timestamp_uS, const T* node)
+        {
+            rp::hal::AutoLocker l(_locker);
+            _data_queue.push_back(*node);
+            if (_data_queue.size() > _max_count) {
+                _data_queue.pop_front();
+            }
+            _data_waiter.set();
+        }
+
+        size_t waitAndFetch(T* node, size_t maxcount, _u32 timeout)
+        {
+            if (_data_waiter.wait(timeout) == rp::hal::Event::EVENT_OK)
+            {
+                rp::hal::AutoLocker l(_locker);
+
+                size_t copiedCount = 0;
+
+                while (maxcount--) {
+                    node[copiedCount++] = _data_queue.front();
+                    _data_queue.pop_front();
+                }
+
+                return copiedCount;
+            }
+            return 0;
+        }
+    protected:
+        size_t          _max_count;
+        rp::hal::Locker _locker;
+        rp::hal::Event  _data_waiter;
+        std::deque<T>   _data_queue;
+        
+    };
+
+    template<typename T>
+    class ScanDataHolder
+    {
+    public:
+        ScanDataHolder(size_t maxcount = 8192) 
+            : _scan_node_buffer_size(maxcount)
+            , _scan_node_available_id(-1)
+            , _new_scan_ready(false)
+        {
+            _scanbuffer[0].reserve(_scan_node_buffer_size);
+            _scanbuffer[1].reserve(_scan_node_buffer_size);
+
+            memset(_scan_begin_timestamp_uS, 0, sizeof(_scan_begin_timestamp_uS));
+        }
+
+        size_t getMaxCacheCount() const {
+            return _scan_node_buffer_size;
+        }
+
+
+        void reset() {
+            rp::hal::AutoLocker l(_locker);
+            _scan_node_available_id = -1;
+            _new_scan_ready = false;
+            _scanbuffer[0].clear();
+            _scanbuffer[1].clear();
+            _data_waiter.set(false);
+            memset(_scan_begin_timestamp_uS, 0, sizeof(_scan_begin_timestamp_uS));
+        }
+
+        bool checkNewScanSignalAndReset()
+        {
+            return _new_scan_ready.exchange(false);
+        }
+
+        void pushScanNodeData(_u64 currentSampleTsUs, const T* hqNode)
+        {
+            rp::hal::AutoLocker l(_locker);
+
+            int  operationBufID = _getOperationBufferID_locked();
+            auto operationalBuf = &_scanbuffer[operationBufID];
+            
+            if (hqNode->flag & RPLIDAR_RESP_HQ_FLAG_SYNCBIT) {
+                if (operationalBuf->size()) {
+                    operationBufID = _finishCurrentScanAndSwap_locked();
+                    operationalBuf = &_scanbuffer[operationBufID];
+
+                    // publish the available scan
+                    _new_scan_ready = true;
+                    _data_waiter.set();
+
+                }
+                
+                assert(operationalBuf->size() == 0);
+
+                //store the timestamp info
+                _scan_begin_timestamp_uS[operationBufID] = currentSampleTsUs;
+            }
+            else {
+                if (operationalBuf->size() == 0) {
+                    //discard the data, do not form partial scan
+                    return;
+                }
+            }
+
+            if (operationalBuf->size() >= _scan_node_buffer_size) {
+                //replace the last entry if buffer is full
+                operationalBuf->at(operationalBuf->size() - 1) = *hqNode;
+            }
+            else {
+                operationalBuf->push_back(*hqNode);
+            }
+
+        }
+
+        void rewindCurrentScanData() {
+            rp::hal::AutoLocker l(_locker);
+            _getOperationalBuffer_locked().clear();
+        }
+
+        std::vector<T>* waitAndLockAvailableScan(_u32 timeout, _u64 * out_timestamp_uS = nullptr)
+        {
+            if (_data_waiter.wait(timeout) == rp::hal::Event::EVENT_OK)
+            {
+                _locker.lock();
+                assert(_scan_node_available_id >= 0);
+                _new_scan_ready = false;
+                if (out_timestamp_uS) {
+                    *out_timestamp_uS = _scan_begin_timestamp_uS[_scan_node_available_id];
+                }
+                return &_scanbuffer[_scan_node_available_id];
+            }
+            else {
+                return nullptr;
+            }
+        }
+
+        void unlockScan(std::vector<T>* scan) {
+            if (scan) {
+                _locker.unlock();
+            }
+        }
+
+    protected:
+        int _finishCurrentScanAndSwap_locked() {
+            _scan_node_available_id = _getOperationBufferID_locked();
+            int newOperationalID  =  1 - _scan_node_available_id;
+
+            _scanbuffer[newOperationalID].clear();
+            return newOperationalID;
+        }
+
+        int _getOperationBufferID_locked() {
+            if (_scan_node_available_id < 0) return 0;
+            return 1 - _scan_node_available_id;
+        }
+
+        std::vector<T>& _getOperationalBuffer_locked()
+        {
+            return _scanbuffer[_getOperationBufferID_locked()];
+        }
+
+
+        rp::hal::Locker _locker;
+        rp::hal::Event  _data_waiter;
+
+        
+
+        _u64   _scan_begin_timestamp_uS[2];
+        size_t _scan_node_buffer_size;
+        int    _scan_node_available_id;
+        std::atomic<bool>   _new_scan_ready;
+
+        std::vector<T> _scanbuffer[2];
+    };
+
+    class SlamtecLidarDriver : 
+        public ILidarDriver, internal::IProtocolMessageListener, internal::LIDARSampleDataListener
+    {
+    public:
+        enum {
+            MAX_SCANNODE_CACHE_COUNT = 8192,
+        };
+
+        enum {
+            A2A3_LIDAR_MINUM_MAJOR_ID  = 2,
+            BUILTIN_MOTORCTL_MINUM_MAJOR_ID = 6,
+        };
+
+
+        enum {
+            TOF_C_SERIAL_MINUM_MAJOR_ID = 4,
+            TOF_S_SERIAL_MINUM_MAJOR_ID = 6,
+            TOF_T_SERIAL_MINUM_MAJOR_ID = 9,
+            TOF_M_SERIAL_MINUM_MAJOR_ID = 12,
+
+
+            NEWDESIGN_MINUM_MAJOR_ID = TOF_C_SERIAL_MINUM_MAJOR_ID,
+        };
+
+    public:
+        SlamtecLidarDriver()
+            : _isConnected(false)
+            , _isSupportingMotorCtrl(MotorCtrlSupportNone)
+            , _op_locker(true)
+            , _scanHolder(MAX_SCANNODE_CACHE_COUNT)
+            , _rawSampleNodeHolder(MAX_SCANNODE_CACHE_COUNT)
+            , _waiting_packet_type(0)
+        {
+            _protocolHandler = std::make_shared< internal::RPLidarProtocolCodec>();
+            _transeiver = std::make_shared< internal::AsyncTransceiver>(*_protocolHandler);
+            _dataunpacker.reset(internal::LIDARSampleDataUnpacker::CreateInstance(*this));
+
+            _protocolHandler->setMessageListener(this);
+
+            memset(&_cached_DevInfo, 0, sizeof(_cached_DevInfo));
+        }
+
+
+        virtual ~SlamtecLidarDriver()
+        {
+            disconnect();
+            _protocolHandler->setMessageListener(nullptr);
+        }
+
+
+        LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!devInfo) {
+                devInfo = &_cached_DevInfo;
+            }
+
+            return ParseLIDARTechnologyTypeByModelID(devInfo->model);
+        }
+
+        LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!devInfo) {
+                devInfo = &_cached_DevInfo;
+            }
+
+            return ParseLIDARMajorTypeByModelID(devInfo->model);
+
+        }
+
+        sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName, const sl_lidar_response_device_info_t* devInfo, sl_u32 timeout)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            u_result ans;
+            // fetch alias (commerical) name if asked:
+            if (fetchAliasName) {
+                std::vector<_u8> replyData;
+                ans = getLidarConf(SL_LIDAR_CONF_MODEL_NAME_ALIAS, replyData, nullptr, 0, timeout);
+                if (IS_OK(ans) && replyData.size()) {
+                    out_description.resize(replyData.size() + 1);
+                    memcpy(&out_description[0], &replyData[0], replyData.size());
+                    out_description[replyData.size()] = '\0';
+                    if (out_description != "") {
+                        return SL_RESULT_OK;
+                    }
+                }
+            }
+
+
+            if (!devInfo) {
+                devInfo = &_cached_DevInfo;
+            }
+
+
+            out_description = GetModelNameStringByModelID(devInfo->model);
+
+            return SL_RESULT_OK;
+        }
+
+        sl_result connect(IChannel* channel)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!channel) return SL_RESULT_OPERATION_FAIL;
+            if (isConnected()) return SL_RESULT_ALREADY_DONE;
+
+            _rawSampleNodeHolder.clear();
+
+            sl_result ans;
+            
+       
+            ans = (sl_result)_transeiver->openChannelAndBind(channel);
+
+            if (IS_OK(ans)) {
+                _isConnected = true;
+                // the first dev info local cache will be taken here
+                checkMotorCtrlSupport(_isSupportingMotorCtrl, 500);
+            }
+            
+            return ans;
+        }
+
+        void disconnect()
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (_isConnected) {
+                _disableDataGrabbing();
+
+                _transeiver->unbindAndClose();
+                _isConnected = false;
+            }
+        }
+
+        bool isConnected()
+        {
+            return _isConnected;
+        }
+
+        sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            // send reset message 
+            return (sl_result)_sendCommandWithoutResponse(SL_LIDAR_CMD_RESET);
+        }
+
+        sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            bool confProtocolSupported = false;
+            ans = checkSupportConfigCommands(confProtocolSupported, timeoutInMs);
+            if (!ans) return SL_RESULT_INVALID_DATA;
+
+            if (confProtocolSupported) {
+                // 1. get scan mode count
+                sl_u16 modeCount;
+                ans = getScanModeCount(modeCount, timeoutInMs);
+                if (!ans) return ans;
+                // 2. for loop to get all fields of each scan mode
+                for (sl_u16 i = 0; i < modeCount; i++) {
+                    LidarScanMode scanModeInfoTmp;
+                    memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp));
+                    scanModeInfoTmp.id = i;
+                    ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i, timeoutInMs);
+                    if (!ans) return ans;
+                    ans = getMaxDistance(scanModeInfoTmp.max_distance, i, timeoutInMs);
+                    if (!ans) return ans;
+                    ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i, timeoutInMs);
+                    if (!ans) return ans;
+                    ans = getScanModeName(scanModeInfoTmp.scan_mode, sizeof(scanModeInfoTmp.scan_mode), i, timeoutInMs);
+                    if (!ans) return ans;
+                    outModes.push_back(scanModeInfoTmp);
+
+                }
+                return ans;
+            }
+
+            return ans;        
+        }
+
+        sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            std::vector<sl_u8> answer;
+            bool lidarSupportConfigCmds = false;
+            ans = checkSupportConfigCommands(lidarSupportConfigCmds);
+            if (!ans) return ans;
+
+            if (lidarSupportConfigCmds) {
+                ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_TYPICAL, answer, nullptr, 0, timeoutInMs);
+                if (!ans) return ans;
+                if (answer.size() < sizeof(sl_u16)) {
+                    return SL_RESULT_INVALID_DATA;
+                }
+                const sl_u16 *p_answer = reinterpret_cast<const sl_u16*>(&answer[0]);
+                outMode = *p_answer;
+                return ans;
+            }
+            //old version of triangle lidar
+            else {
+                outMode = SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS;
+                return ans;
+            }
+            return ans;
+    
+        }
+
+        sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            bool ifSupportLidarConf = false;
+            LidarScanMode localMode;
+
+            if (!isConnected()) return SL_RESULT_OPERATION_FAIL;
+            stop();
+
+            if (!outUsedScanMode) outUsedScanMode = &localMode;
+
+
+            ans = checkSupportConfigCommands(ifSupportLidarConf);
+            if (!ans) return ans;
+            if (useTypicalScan){
+                sl_u16 typicalMode;
+                ans = getTypicalScanMode(typicalMode);
+                if (!ans) return ans;
+
+                //call startScanExpress to do the job
+                return startScanExpress(false, typicalMode, 0, outUsedScanMode);
+            }
+
+            // 'useTypicalScan' is false, just use normal scan mode
+
+
+            return startScanNormal_commonpath(force, ifSupportLidarConf , *outUsedScanMode, DEFAULT_TIMEOUT);
+        }
+
+
+        // this path make sure the working mode has always been retrieved
+        sl_result startScanNormal_commonpath(bool force, bool ifSupportLidarConf, LidarScanMode& outUsedScanMode, sl_u32 timeout)
+        {
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+
+            if (ifSupportLidarConf) {
+
+                outUsedScanMode.id = SL_LIDAR_CONF_SCAN_COMMAND_STD;
+                ans = getLidarSampleDuration(outUsedScanMode.us_per_sample, outUsedScanMode.id);
+                if (!ans) return ans;
+                ans = getMaxDistance(outUsedScanMode.max_distance, outUsedScanMode.id);
+                if (!ans) return ans;
+                ans = getScanModeAnsType(outUsedScanMode.ans_type, outUsedScanMode.id);
+                if (!ans) return ans;
+                ans = getScanModeName(outUsedScanMode.scan_mode, sizeof(outUsedScanMode.scan_mode), outUsedScanMode.id);
+                if (!ans) return ans;
+
+            }
+            else {
+                // a legacy device
+                rplidar_response_sample_rate_t sampleRateTmp;
+                ans = _getLegacySampleDuration_uS(sampleRateTmp, timeout);
+
+                if (!ans) return SL_RESULT_INVALID_DATA;
+                outUsedScanMode.us_per_sample = sampleRateTmp.std_sample_duration_us;
+                outUsedScanMode.max_distance = 16;
+                outUsedScanMode.ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT;
+                strcpy(outUsedScanMode.scan_mode, "Standard");
+            }
+
+
+            _updateTimingDesc(_cached_DevInfo, outUsedScanMode.us_per_sample);
+
+            startMotor();
+
+            _scanHolder.reset();
+            _dataunpacker->enable();
+
+            ans = _sendCommandWithoutResponse(force ? SL_LIDAR_CMD_FORCE_SCAN : SL_LIDAR_CMD_SCAN, nullptr, 0, true);
+            if (ans) delay(10); // wait rplidar to handle it
+            return ans;
+        }
+
+
+        sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            LidarScanMode localMode;
+            bool ifSupportLidarConf;
+
+            if (!isConnected()) return SL_RESULT_OPERATION_FAIL;
+            stop();
+
+            Result<nullptr_t> ans = checkSupportConfigCommands(ifSupportLidarConf);
+            if (!ans) return ans;
+
+            return startScanNormal_commonpath(force, ifSupportLidarConf, localMode, timeout);
+        }
+
+        sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            if (!isConnected()) return SL_RESULT_OPERATION_FAIL;
+            stop(); //force the previous operation to stop
+
+            LidarScanMode localMode;
+
+            if (!outUsedScanMode) outUsedScanMode = &localMode;
+
+            bool ifSupportLidarConf = false;
+            ans = checkSupportConfigCommands(ifSupportLidarConf);
+            if (!ans) return SL_RESULT_INVALID_DATA;
+
+
+            
+            outUsedScanMode->id = scanMode;
+            if (ifSupportLidarConf) {
+                ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id);
+                if (!ans) return SL_RESULT_INVALID_DATA;
+
+                ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id);
+                if (!ans) return SL_RESULT_INVALID_DATA;
+
+                ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id);
+                if (!ans) return SL_RESULT_INVALID_DATA;
+
+                ans = getScanModeName(outUsedScanMode->scan_mode, sizeof(outUsedScanMode->scan_mode), outUsedScanMode->id);
+                if (!ans) return SL_RESULT_INVALID_DATA;
+            }
+            else {
+                // legacy device support
+                if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD) {
+                    rplidar_response_sample_rate_t sampleRateTmp;
+                    ans = _getLegacySampleDuration_uS(sampleRateTmp, timeout);
+                    if (!ans) return RESULT_INVALID_DATA;
+
+                    outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us;
+                    outUsedScanMode->max_distance = 16;
+                    outUsedScanMode->ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
+                    strcpy(outUsedScanMode->scan_mode, "Express");
+                }
+                else {
+                    outUsedScanMode->ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT;
+                }
+            }
+
+            if (outUsedScanMode->ans_type == SL_LIDAR_ANS_TYPE_MEASUREMENT)
+            {
+                // redirect to the correct function...
+                return startScanNormal(force, timeout);
+            }
+            
+            _updateTimingDesc(_cached_DevInfo, outUsedScanMode->us_per_sample);
+            startMotor();
+
+            _scanHolder.reset();
+            _dataunpacker->enable();
+
+            sl_lidar_payload_express_scan_t scanReq;
+            memset(&scanReq, 0, sizeof(scanReq));
+
+            if (!ifSupportLidarConf) {
+                if (scanMode != SL_LIDAR_CONF_SCAN_COMMAND_STD && scanMode != SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS)
+                    scanReq.working_mode = sl_u8(scanMode);
+            }
+            else
+                scanReq.working_mode = sl_u8(scanMode);
+
+            scanReq.working_flags = options;
+
+            ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq), true);
+            if (ans) delay(10); // wait rplidar to handle it
+            return ans;
+
+        }
+
+        sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+
+
+            u_result ans = SL_RESULT_OK;
+            ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_STOP);
+            _disableDataGrabbing();
+
+            if (IS_FAIL(ans)) return ans;
+            
+
+            delay(100);
+
+            if(_isSupportingMotorCtrl == MotorCtrlSupportPwm)
+                setMotorSpeed(0);
+  
+            return SL_RESULT_OK;
+        }
+
+        sl_result grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64& timestamp_uS, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+
+            if (!nodebuffer)
+                return SL_RESULT_INVALID_DATA;
+
+            auto availBuffer = _scanHolder.waitAndLockAvailableScan(timeout, &timestamp_uS);
+            if (!availBuffer) return SL_RESULT_OPERATION_TIMEOUT;
+
+            count = std::min<size_t>(count, availBuffer->size());
+
+            std::copy(availBuffer->begin(), availBuffer->begin() + count, nodebuffer);
+
+            _scanHolder.unlockScan(availBuffer);
+
+            return RESULT_OK;
+        }
+
+        sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            _u64 localTS;
+            return grabScanDataHqWithTimeStamp(nodebuffer, count, localTS, timeout);
+        }
+
+        sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            u_result ans;
+            internal::message_autoptr_t ans_frame;
+
+            ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_DEVICE_INFO, SL_LIDAR_ANS_TYPE_DEVINFO, ans_frame, timeout);
+
+            if (IS_FAIL(ans)) return ans;
+            if (ans_frame->getPayloadSize() < sizeof(rplidar_response_device_info_t))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            info = *(rplidar_response_device_info_t*)ans_frame->getDataBuf();
+#ifdef _CPU_ENDIAN_BIG
+            info.firmware_version = le16_to_cpu(info.firmware_version);
+#endif
+
+            _cached_DevInfo = info;
+            return (sl_result)ans;
+        }
+
+        sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            support = MotorCtrlSupportNone;
+            _disableDataGrabbing();
+
+            {
+                sl_lidar_response_device_info_t devInfo;
+                ans = getDeviceInfo(devInfo, 500);
+                if (!ans) return ans;
+                sl_u8 majorId = devInfo.model >> 4;
+                if (majorId >= BUILTIN_MOTORCTL_MINUM_MAJOR_ID) {
+                        support = MotorCtrlSupportRpm;
+                        return ans;
+                }
+                else if(majorId >= A2A3_LIDAR_MINUM_MAJOR_ID){
+
+                    rp::hal::AutoLocker l(_op_locker);
+                    sl_lidar_payload_acc_board_flag_t flag;
+                    flag.reserved = 0;
+                    internal::message_autoptr_t ans_frame;
+
+                    ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_ACC_BOARD_FLAG, SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG, ans_frame, timeout, &flag, sizeof(flag));
+                    if (!ans) return ans;
+
+                    if (ans_frame->getPayloadSize() < sizeof(rplidar_response_acc_board_flag_t))
+                    {
+                        return RESULT_INVALID_DATA;
+                    }
+
+                    const sl_lidar_response_acc_board_flag_t* acc_board_flag
+                        = reinterpret_cast<const sl_lidar_response_acc_board_flag_t*>(ans_frame->getDataBuf());
+
+                    if (acc_board_flag->support_flag & SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) {
+                        support = MotorCtrlSupportPwm;
+                    }
+                    return ans;
+                }
+
+            }
+            return SL_RESULT_OK;
+
+        }
+
+        sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency)
+        {
+            float sample_duration = scanMode.us_per_sample;
+            frequency = 1000000.0f / (count * sample_duration);
+            return SL_RESULT_OK;
+        }
+
+		sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout)
+		{
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+			sl_result ans = setLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, &conf, sizeof(sl_lidar_ip_conf_t), timeout);
+			return ans;
+		}
+
+        sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            std::vector<sl_u8> reserve(2); //keep backward compatibility
+
+            std::vector<sl_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, answer, &reserve[0], 2, timeout);
+            size_t len = answer.size();
+            if (0 == len) return SL_RESULT_INVALID_DATA;
+            memcpy(&conf, &answer[0], len);
+            return ans;
+        }
+       
+        sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            u_result ans;
+            internal::message_autoptr_t ans_frame;
+
+            ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_DEVICE_HEALTH, SL_LIDAR_ANS_TYPE_DEVHEALTH, ans_frame, timeout);
+
+            if (IS_FAIL(ans)) return ans;
+            if (ans_frame->getPayloadSize() < sizeof(rplidar_response_device_health_t))
+            {
+                return SL_RESULT_INVALID_DATA;
+            }
+            health = *(rplidar_response_device_health_t*)ans_frame->getDataBuf();
+#ifdef _CPU_ENDIAN_BIG
+            health.error_code = le16_to_cpu(health.error_code);
+#endif
+
+            return ans;
+        }
+
+		sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs)
+		{
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            u_result ans;
+
+            std::vector<_u8> answer(6, 0);
+            ans = getLidarConf(SL_LIDAR_CONF_LIDAR_MAC_ADDR, answer, NULL, 0, timeoutInMs);
+            if (IS_FAIL(ans))
+            {
+                return ans;
+            }
+            size_t len = answer.size();
+            if (0 == len) return SL_RESULT_INVALID_DATA;
+            memcpy(macAddrArray, &answer[0], len);
+            return ans;
+		}
+
+        sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count)
+        {
+            return ascendScanData_<sl_lidar_response_measurement_node_hq_t>(nodebuffer, count);
+        }
+
+        sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count)
+        {
+            count = _rawSampleNodeHolder.waitAndFetch(nodebuffer, count, 0);
+            return SL_RESULT_OK;
+        }
+
+        sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED)
+        {
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            
+            if(speed == DEFAULT_MOTOR_SPEED){
+                sl_lidar_response_desired_rot_speed_t desired_speed;
+                ans = getDesiredSpeed(desired_speed);
+                if (ans) {
+                    if (_isSupportingMotorCtrl == MotorCtrlSupportPwm)
+                        speed = desired_speed.pwm_ref;
+                    else
+                        speed = desired_speed.rpm;
+                }
+                else {
+                    //set a dummy default value
+                    speed = 600;
+                }
+            }
+            switch (_isSupportingMotorCtrl)
+            {
+            case MotorCtrlSupportNone:
+                if (_transeiver->getBindedChannel()->getChannelType() == CHANNEL_TYPE_SERIALPORT) {
+                    ISerialPortChannel* serialChanel = (ISerialPortChannel*)_transeiver->getBindedChannel();
+                    if (!speed) {
+                        serialChanel->setDTR(true);
+                    }else{
+                        serialChanel->setDTR(false);
+                    }
+                }
+                break;
+            case MotorCtrlSupportPwm:
+                sl_lidar_payload_motor_pwm_t motor_pwm;
+                motor_pwm.pwm_value = speed;
+
+
+                ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_SET_MOTOR_PWM, &motor_pwm, sizeof(motor_pwm), true);
+                if (!ans) return ans;
+                delay(10);
+                break;
+            case MotorCtrlSupportRpm:
+                sl_lidar_payload_motor_pwm_t motor_rpm;
+                motor_rpm.pwm_value = speed;
+
+                ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL, &motor_rpm, sizeof(motor_rpm), true);
+                if (!ans) return ans;
+                delay(10);
+                break;
+            }
+            return SL_RESULT_OK;
+        }
+
+        sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs)
+        {
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            rp::hal::AutoLocker l(_op_locker);
+            if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT;
+
+
+            {
+                std::vector<sl_u8> answer;
+
+			    ans = getLidarConf(SL_LIDAR_CONF_MIN_ROT_FREQ, answer);
+			    if (!ans) return ans;
+
+			    const sl_u16 *min_answer = reinterpret_cast<const sl_u16*>(&answer[0]);
+                motorInfo.min_speed = *min_answer;
+
+
+                ans = getLidarConf(SL_LIDAR_CONF_MAX_ROT_FREQ, answer);
+			    if (!ans) return ans;
+
+			    const sl_u16 *max_answer = reinterpret_cast<const sl_u16*>(&answer[0]);
+                motorInfo.max_speed = *max_answer;
+
+                sl_lidar_response_desired_rot_speed_t desired_speed;
+                ans = getDesiredSpeed(desired_speed);
+                if (!ans) return ans;
+                if(motorInfo.motorCtrlSupport == MotorCtrlSupportPwm)
+                    motorInfo.desired_speed = desired_speed.pwm_ref;
+                else
+                    motorInfo.desired_speed = desired_speed.rpm;
+
+            }
+            return SL_RESULT_OK;
+        }
+
+        sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected)
+        {
+            // ask the LIDAR to stop working first...
+            stop();
+
+            rp::hal::AutoLocker l(_op_locker);
+
+            IChannel* cachedChannel = _transeiver->getBindedChannel();
+            if (!cachedChannel) return SL_RESULT_OPERATION_FAIL;
+            if (cachedChannel->getChannelType() != CHANNEL_TYPE_SERIALPORT)
+            {
+                // only works for UART connection
+                return RESULT_OPERATION_NOT_SUPPORT;
+            }
+
+            // disable the transeiver as it may interrupt the operation...
+            _transeiver->unbindAndClose();
+
+            sl_result ans = SL_RESULT_OK;
+
+            do {
+                // reopen the channel...
+
+                if (!cachedChannel->open()) {
+                    // failed to reopen
+                    // try to revert back...
+                    ans = SL_RESULT_OPERATION_FAIL;
+                    break;
+                }
+
+                cachedChannel->flush();
+
+                // wait for a while
+                delay(10);
+                cachedChannel->clearReadCache();
+
+                // sending magic byte to let the target LIDAR start baudrate measurement
+                // More than 100 bytes per second datarate is required to trigger the measurements
+                {
+
+
+                    sl_u8 magicByteSeq[16];
+
+                    memset(magicByteSeq, SL_LIDAR_AUTOBAUD_MAGICBYTE, sizeof(magicByteSeq));
+
+                    sl_u64 startTS = getms();
+
+                    while (getms() - startTS < 1500) //lasting for 1.5sec
+                    {
+                        if (cachedChannel->write(magicByteSeq, sizeof(magicByteSeq)) < 0)
+                        {
+                            ans = SL_RESULT_OPERATION_FAIL;
+                            break;
+                        }
+
+                        size_t dataCountGot;
+                        if (cachedChannel->waitForData(1, 1, &dataCountGot)) {
+                            //got reply, stop
+                            ans = SL_RESULT_OK;
+                            break;
+                        }
+                    }
+                }
+
+                if (IS_FAIL(ans)) break;
+
+                // getback the bps measured
+                _u32 bpsDetected = 0;
+                size_t dataCountGot;
+                if (cachedChannel->waitForData(4, 500, &dataCountGot)) {
+                    //got reply, stop
+                    cachedChannel->read(&bpsDetected, 4);
+                    if (baudRateDetected) *baudRateDetected = bpsDetected;
+
+
+                    cachedChannel->close();
+                    // restart the transiever 
+                    ans = _transeiver->openChannelAndBind(cachedChannel);
+                    if (IS_FAIL(ans)) return ans;
+
+
+                    // send a confirmation to the LIDAR, otherwise, the previous baudrate will be reverted back
+                    sl_lidar_payload_new_bps_confirmation_t confirmation;
+                    confirmation.flag = 0x5F5F;
+                    confirmation.required_bps = requiredBaudRate;
+                    confirmation.param = 0;
+
+
+                    ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM, &confirmation, sizeof(confirmation));
+
+                    return ans;
+                }
+            } while (0);
+
+            _transeiver->openChannelAndBind(cachedChannel);
+
+            return ans;
+        }
+
+    protected:
+        sl_result startMotor()
+        {
+            return setMotorSpeed(DEFAULT_MOTOR_SPEED);
+        }
+
+        u_result getDesiredSpeed(sl_lidar_response_desired_rot_speed_t & motorSpeed, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+            std::vector<sl_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_DESIRED_ROT_FREQ, answer, nullptr, 0, timeoutInMs);
+
+            if (IS_FAIL(ans)) return ans;
+
+            const sl_lidar_response_desired_rot_speed_t *p_answer = reinterpret_cast<const sl_lidar_response_desired_rot_speed_t*>(&answer[0]);
+            motorSpeed = *p_answer;
+            return RESULT_OK;
+        }
+
+        u_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+            rplidar_response_device_info_t devinfo;
+            ans = getDeviceInfo(devinfo, timeoutInMs);
+            if (IS_FAIL(ans)) {
+                outSupport = false;
+                return ans;
+            }
+
+
+            if (_checkNDMagicNumber(devinfo.model)) {
+
+                outSupport = true;
+            }
+            else {
+                // if lidar firmware >= 1.24
+                outSupport = (devinfo.firmware_version >= ((0x1 << 8) | 24));
+            }
+            return RESULT_OK;
+        }
+
+
+        u_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+            std::vector<_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_COUNT, answer);
+            if (IS_FAIL(ans)) {
+                return ans;
+            }
+            if (answer.size() < sizeof(_u16)) {
+                return RESULT_INVALID_DATA;
+            }
+            const _u16* p_answer = reinterpret_cast<const _u16*>(&answer[0]);
+            modeCount = *p_answer;
+            return ans;
+        }
+
+        u_result setLidarConf(_u32 type, const void* payload, size_t payloadSize, _u32 timeout)
+		{
+			if (type < 0x00010000 || type >0x0001FFFF)
+				return SL_RESULT_INVALID_DATA;
+
+
+			std::vector<sl_u8> requestPkt;
+			requestPkt.resize(sizeof(sl_lidar_payload_set_scan_conf_t) + payloadSize);
+			if (!payload) payloadSize = 0;
+			sl_lidar_payload_set_scan_conf_t* query = reinterpret_cast<sl_lidar_payload_set_scan_conf_t*>(&requestPkt[0]);
+
+			query->type = type;
+
+			if (payloadSize)
+				memcpy(&query[1], payload, payloadSize);
+
+			sl_result ans;
+            internal::message_autoptr_t ans_frame;
+            ans = _sendCommandWithResponse(SL_LIDAR_CMD_SET_LIDAR_CONF, SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF, ans_frame, timeout, &requestPkt[0], requestPkt.size());
+
+            if (IS_FAIL(ans)) {
+                return ans;
+            }
+
+            //check if returned size is even less than sizeof(type) 
+            if (ans_frame->getPayloadSize() < sizeof(rplidar_response_set_lidar_conf_t)) {
+                return RESULT_INVALID_DATA;
+            }
+
+            const rplidar_response_set_lidar_conf_t* response =
+                reinterpret_cast<const rplidar_response_set_lidar_conf_t*>(ans_frame->getDataBuf());
+
+
+            if (ans_frame->getPayloadSize() == 4) {
+                // legacy device?
+                return *(const u_result*)(ans_frame->getDataBuf());
+            }
+            else {
+                if (response->type != type) {
+                    return RESULT_INVALID_DATA;
+                }
+
+                return (u_result)response->result;
+            }
+		}
+
+        u_result getLidarConf(_u32 type, std::vector<_u8>& outputBuf, const void* payload = NULL, size_t payloadSize = 0, _u32 timeout = DEFAULT_TIMEOUT)
+        {
+            std::vector<_u8> requestPkt;
+
+            if (!payload) payloadSize = 0;
+            requestPkt.resize(sizeof(rplidar_payload_get_scan_conf_t) + payloadSize);
+            rplidar_payload_get_scan_conf_t* query = reinterpret_cast<rplidar_payload_get_scan_conf_t*>(&requestPkt[0]);
+
+            query->type = type;
+
+            if (payloadSize)
+                memcpy(&query[1], payload, payloadSize);
+
+            u_result ans;
+            internal::message_autoptr_t ans_frame;
+            ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_LIDAR_CONF, SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF, ans_frame, timeout, &requestPkt[0], requestPkt.size());
+            if (IS_FAIL(ans)) {
+                return ans;
+            }
+            //check if returned size is even less than sizeof(type) 
+            if (ans_frame->getPayloadSize() < offsetof(rplidar_response_get_lidar_conf_t, payload)) {
+                return SL_RESULT_INVALID_DATA;
+            }
+
+            //check if returned type is same as asked type
+            const rplidar_response_get_lidar_conf_t* replied =
+                reinterpret_cast<const rplidar_response_get_lidar_conf_t*>(ans_frame->getDataBuf());
+
+
+            if (replied->type != type) {
+                return SL_RESULT_INVALID_DATA;
+            }
+            //copy all the payload into &outputBuf
+            int payLoadLen = (int)ans_frame->getPayloadSize() - (int)offsetof(rplidar_response_get_lidar_conf_t, payload);
+            //do consistency check
+            if (payLoadLen < 0) {
+                return SL_RESULT_INVALID_DATA;
+            }
+            //copy all payLoadLen bytes to outputBuf
+            outputBuf.resize(payLoadLen);
+            if (payLoadLen)
+                memcpy(&outputBuf[0], replied->payload, payLoadLen);
+            return ans;
+        }
+
+        u_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+
+            std::vector<_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, &scanModeID, sizeof(_u16), timeoutInMs);
+            if (IS_FAIL(ans))
+            {
+                return ans;
+            }
+            if (answer.size() < sizeof(_u32))
+            {
+                return SL_RESULT_INVALID_DATA;
+            }
+            const _u32* result = reinterpret_cast<const _u32*>(&answer[0]);
+            sampleDurationRes = (float)(*result / 256.0);
+            return ans;
+        }
+
+        u_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+
+
+            std::vector<_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, &scanModeID, sizeof(_u16), timeoutInMs);
+            if (IS_FAIL(ans))
+            {
+                return ans;
+            }
+            if (answer.size() < sizeof(_u32))
+            {
+                return SL_RESULT_INVALID_DATA;
+            }
+            const _u32* result = reinterpret_cast<const _u32*>(&answer[0]);
+            maxDistance = (float)(*result >> 8);
+            return ans;
+        }
+
+        u_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+
+            std::vector<_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, &scanModeID, sizeof(_u16), timeoutInMs);
+            if (IS_FAIL(ans))
+            {
+                return ans;
+            }
+            if (answer.size() < sizeof(_u8))
+            {
+                return SL_RESULT_INVALID_DATA;
+            }
+            const _u8* result = reinterpret_cast<const _u8*>(&answer[0]);
+            ansType = *result;
+            return ans;
+        }
+
+        u_result getScanModeName(char* modeName, size_t stringSize, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT)
+        {
+            u_result ans;
+
+            std::vector<_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_NAME, answer, &scanModeID, sizeof(_u16), timeoutInMs);
+            if (IS_FAIL(ans))
+            {
+                return ans;
+            }
+            size_t len = std::min<size_t>(answer.size(), stringSize);
+            if (0 == len) return SL_RESULT_INVALID_DATA;
+
+            memcpy(modeName, &answer[0], len);
+            return ans;
+        }
+
+
+        static LIDARTechnologyType ParseLIDARTechnologyTypeByModelID(_u8 modelID)
+        {
+            _u8 majorModelID = (modelID >> 4);
+            // FIXME: stupid implementation here
+            if (majorModelID < NEWDESIGN_MINUM_MAJOR_ID) {
+                return LIDAR_TECHNOLOGY_TRIANGULATION;
+            }
+            else {
+                return LIDAR_TECHNOLOGY_DTOF;
+            }
+        }
+
+        static LIDARMajorType ParseLIDARMajorTypeByModelID(_u8 modelID)
+        {
+            _u8 majorModelID = (modelID >> 4);
+
+
+            if (majorModelID >= TOF_M_SERIAL_MINUM_MAJOR_ID) {
+                return LIDAR_MAJOR_TYPE_M_SERIES;
+            }
+            else if (majorModelID >= TOF_T_SERIAL_MINUM_MAJOR_ID) {
+                return LIDAR_MAJOR_TYPE_T_SERIES;
+            }
+            else if (majorModelID >= TOF_S_SERIAL_MINUM_MAJOR_ID) {
+                return LIDAR_MAJOR_TYPE_S_SERIES;
+            }
+            else if (majorModelID >= TOF_C_SERIAL_MINUM_MAJOR_ID) {
+                return LIDAR_MAJOR_TYPE_C_SERIES;
+            }
+            else {
+                return LIDAR_MAJOR_TYPE_A_SERIES;
+            }
+        }
+
+        static std::string         GetModelNameStringByModelID(_u8 modelID)
+        {
+
+            char stringBuffer[100];
+            auto majorType = ParseLIDARMajorTypeByModelID(modelID);
+
+
+            switch (majorType) {
+            case LIDAR_MAJOR_TYPE_A_SERIES:
+                sprintf(stringBuffer, "A%dM%d", (modelID >> 4), (modelID & 0xF));
+
+                break;
+
+            case LIDAR_MAJOR_TYPE_S_SERIES:
+                sprintf(stringBuffer, "S%dM%d", (modelID >> 4) - (TOF_S_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF));
+
+                break;
+
+            case LIDAR_MAJOR_TYPE_T_SERIES:
+                sprintf(stringBuffer, "T%dM%d", (modelID >> 4) - (TOF_T_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF));
+
+                break;
+
+            case LIDAR_MAJOR_TYPE_M_SERIES:
+                sprintf(stringBuffer, "M%dM%d", (modelID >> 4) - (TOF_M_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF));
+
+                break;
+
+            case LIDAR_MAJOR_TYPE_C_SERIES:
+                sprintf(stringBuffer, "C%dM%d", (modelID >> 4) - (TOF_C_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF));
+
+                break;
+
+
+            default:
+                sprintf(stringBuffer, "unknown(%x)", modelID);
+            }
+
+            return std::string(stringBuffer);
+        }
+
+    protected:
+        
+        void _disableDataGrabbing()
+        {
+            _dataunpacker->disable();
+            _protocolHandler->exitLoopMode(); // exit loop mode
+        }
+        
+
+   
+        bool _checkNDMagicNumber(_u8 model)
+        {
+            return ((model >> 4) >= NEWDESIGN_MINUM_MAJOR_ID);
+        }
+
+
+
+
+        u_result _detectLIDARNativeInterfaceType(LIDARInterfaceType & outputType, const rplidar_response_device_info_t& devInfo, sl_u32 timeout = DEFAULT_TIMEOUT)
+        {
+            
+            LIDARMajorType majorType = ParseLIDARMajorTypeByModelID(devInfo.model);
+            
+            switch (majorType) {
+            case LIDAR_MAJOR_TYPE_A_SERIES:
+            case LIDAR_MAJOR_TYPE_M_SERIES:
+            case LIDAR_MAJOR_TYPE_C_SERIES:
+          
+                outputType = LIDAR_INTERFACE_UART;
+                return SL_RESULT_OK;
+
+
+            case LIDAR_MAJOR_TYPE_T_SERIES:
+                outputType = LIDAR_INTERFACE_ETHERNET;
+                return SL_RESULT_OK;
+
+            case LIDAR_MAJOR_TYPE_S_SERIES:
+            {
+                // ethernet version exists, check whether it is
+                _u8 macAddr[6];
+                u_result ans = getDeviceMacAddr(macAddr, timeout);
+                if (IS_FAIL(ans)) {
+                    // cannot retrieve the device mac address, consider a UART interface version
+                    outputType = LIDAR_INTERFACE_UART;
+                }
+                else {
+                    outputType = LIDAR_INTERFACE_ETHERNET;
+                }
+                return SL_RESULT_OK;
+            }
+
+
+            case LIDAR_MAJOR_TYPE_UNKNOWN:
+            default:
+                outputType = LIDAR_INTERFACE_UNKNOWN;
+                return SL_RESULT_OK;
+            }
+        }
+
+        _u32 _getNativeBaudRate(const rplidar_response_device_info_t & devInfo)
+        {
+            _u8 majorModelID = (devInfo.model >> 4);
+            switch (majorModelID)
+            {
+            case 1:
+            case 2:
+            case 3: //A1..A3 series
+                return (devInfo.hardware_version >= 6) ? 256000 : 115200;
+            case 4: //C series
+                return 460800;
+            case 6: //model ID of S1
+                return 256000;
+            case 7: //model ID of S2
+            case 8: //model ID of S3
+                if (devInfo.model == (0x82)) return 460800; 
+                return 1000000;
+            default:
+                return 0; //0 as unknown
+            }
+        }
+
+        bool _updateTimingDesc(const rplidar_response_device_info_t& devInfo, float selectedSampleDuration)
+        {
+            _timing_desc.native_baudrate = _getNativeBaudRate(devInfo);
+            _detectLIDARNativeInterfaceType(_timing_desc.native_interface_type, devInfo, 500);
+            
+            _timing_desc.sample_duration_uS = (_u64)(selectedSampleDuration + 0.5f);
+
+            //FIXME: will be changed in future releases
+            _timing_desc.native_timestamp_support = false; 
+            _timing_desc.linkage_delay_uS = 0;
+
+
+            // notify the data unpacker
+            _dataunpacker->updateUnpackerContext(internal::LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING ,&_timing_desc, sizeof(_timing_desc));
+            return true;
+
+        }
+
+        u_result _getLegacySampleDuration_uS(rplidar_response_sample_rate_t& rateInfo, _u32 timeout)
+        {
+            
+            static const _u32 LEGACY_SAMPLE_DURATION = 476;
+
+            rplidar_response_device_info_t devinfo;
+            // 1. fetch the device version first...
+            u_result ans = getDeviceInfo(devinfo, timeout);
+
+            rateInfo.express_sample_duration_us = LEGACY_SAMPLE_DURATION;
+            rateInfo.std_sample_duration_us = LEGACY_SAMPLE_DURATION;
+
+            if (IS_FAIL(ans)) {
+                return ans;
+            }
+
+            if (getLIDARMajorType(&devinfo) == LIDAR_MAJOR_TYPE_A_SERIES) {
+                if (devinfo.firmware_version < ((0x1 << 8) | 17)) {
+                    // very very rare and old model found!!
+                    return SL_RESULT_OK;
+                }
+            }
+
+
+            internal::message_autoptr_t ans_frame;
+
+            ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_SAMPLERATE, SL_LIDAR_ANS_TYPE_SAMPLE_RATE, ans_frame, timeout);
+
+            if (IS_FAIL(ans)) return ans;
+            if (ans_frame->getPayloadSize() < sizeof(rplidar_response_sample_rate_t))
+            {
+                return RESULT_INVALID_DATA;
+            }
+            memcpy(&rateInfo, ans_frame->getDataBuf(), sizeof(rateInfo));
+
+#ifdef _CPU_ENDIAN_BIG
+            rateInfo.express_sample_duration_us = le16_to_cpu(rateInfo.express_sample_duration_us);
+            rateInfo.std_sample_duration_us = le16_to_cpu(rateInfo.std_sample_duration_us);
+#endif
+
+            return ans;
+        }
+
+
+        u_result _sendCommandWithoutResponse(_u8 cmd, const void* payload = NULL, size_t payloadsize = 0, bool noForceStop = false)
+        {
+            if (!noForceStop) {
+                _disableDataGrabbing();
+            }
+            _response_waiter.set(false);
+
+            internal::message_autoptr_t message(new internal::ProtocolMessage(cmd, (const _u8*)payload, payloadsize));
+            return _transeiver->sendMessage(message);
+
+        }
+
+        u_result _sendCommandWithResponse(_u8 cmd, _u8 responseType, internal::message_autoptr_t& ansPkt, _u32 timeout = DEFAULT_TIMEOUT, const void* payload = NULL, size_t payloadsize = 0)
+        {
+            u_result ans;
+
+            _data_locker.lock();
+
+            internal::message_autoptr_t message(new internal::ProtocolMessage(cmd, (const _u8*)payload, payloadsize));
+            _disableDataGrabbing();
+            _waiting_packet_type = responseType;
+            _response_waiter.set(false);
+            _data_locker.unlock();
+
+            ans = _transeiver->sendMessage(message);
+
+            if (IS_FAIL(ans)) return ans;
+
+            do {
+                switch (_response_waiter.wait(timeout)) {
+                case rp::hal::Event::EVENT_TIMEOUT:
+                    return RESULT_OPERATION_TIMEOUT;
+                case rp::hal::Event::EVENT_OK:
+                    _data_locker.lock();
+                    ansPkt = _lastAnsPkt;
+                    _data_locker.unlock();
+                    return RESULT_OK;
+                default:
+                    return RESULT_OPERATION_FAIL;
+                }
+            } while (1);
+        }
+        
+    public:
+
+        virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node)
+        {
+            _scanHolder.pushScanNodeData(timestamp_uS, node);
+            _rawSampleNodeHolder.pushNode(timestamp_uS, node);
+        }
+
+        virtual void onHQNodeScanResetReq() {
+            _scanHolder.rewindCurrentScanData();
+        }
+
+        virtual void onProtocolMessageDecoded(const internal::ProtocolMessage& msg)
+        {
+            internal::message_autoptr_t message = std::make_shared<internal::ProtocolMessage>(msg);
+
+            if (_dataunpacker->onSampleData(message->cmd, message->getDataBuf(), message->getPayloadSize()))
+            {
+                return;
+            }
+
+            if (message->cmd == _waiting_packet_type) {
+                _data_locker.lock();
+                _lastAnsPkt = message;
+                _response_waiter.setResult(message->cmd);
+                _data_locker.unlock();
+            }
+
+            
+        }
+    private:
+
+        std::shared_ptr<internal::RPLidarProtocolCodec> _protocolHandler;
+        std::shared_ptr<internal::AsyncTransceiver> _transeiver;
+        std::shared_ptr<internal::LIDARSampleDataUnpacker> _dataunpacker;
+
+        bool _isConnected;
+
+        MotorCtrlSupport          _isSupportingMotorCtrl;
+
+
+        rp::hal::Locker           _op_locker;
+        rp::hal::Locker           _data_locker;
+        rp::hal::Waiter<_u32>     _response_waiter;
+
+        ScanDataHolder<sl_lidar_response_measurement_node_hq_t> _scanHolder;
+        RawSampleNodeHolder<sl_lidar_response_measurement_node_hq_t> _rawSampleNodeHolder;
+        _u32                          _waiting_packet_type;
+        internal::message_autoptr_t   _lastAnsPkt;
+
+        sl_lidar_response_device_info_t _cached_DevInfo;
+        SlamtecLidarTimingDesc         _timing_desc;
+
+    };
+
+    Result<ILidarDriver*> createLidarDriver()
+    {
+        return new SlamtecLidarDriver();
+    }
+}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_lidarprotocol_codec.cpp b/LIDAR/sdk/src/sl_lidarprotocol_codec.cpp
new file mode 100644
index 0000000..a629000
--- /dev/null
+++ b/LIDAR/sdk/src/sl_lidarprotocol_codec.cpp
@@ -0,0 +1,238 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/byteorder.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+#include "sl_async_transceiver.h"
+#include "sl_lidarprotocol_codec.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+RPLidarProtocolCodec::RPLidarProtocolCodec()
+    : IAsyncProtocolCodec()
+    , _listener(NULL)
+    , _op_locker(true)
+{
+    onDecodeReset();
+}
+
+void RPLidarProtocolCodec::exitLoopMode() {
+    onDecodeReset();
+}
+
+
+
+void RPLidarProtocolCodec::setMessageListener(IProtocolMessageListener* listener)
+{
+    rp::hal::AutoLocker l(_op_locker);
+    _listener = listener;
+}
+
+size_t RPLidarProtocolCodec::estimateLength(message_autoptr_t& message)
+{
+    size_t actualSize = 2; //1-byte's sync byte, 1-byte's cmd byte
+
+    if (message->cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
+        actualSize += (message->getPayloadSize() & 0xFF);
+        actualSize += 2; //1-byte for size field, 1-byte for checksum
+    }
+
+    return actualSize;
+}
+
+
+void RPLidarProtocolCodec::onEncodeData(message_autoptr_t& message, _u8* buffer, size_t* size)
+{
+    _u8 checksum = 0;
+    size_t writeSize = std::min<size_t>(*size, estimateLength(message));
+    size_t currentPos = 0;
+
+    while (currentPos < writeSize) {
+        _u8 currentTxByte;
+        switch (currentPos) {
+        case 0: // sync byte
+            currentTxByte = RPLIDAR_CMD_SYNC_BYTE;
+            break;
+        case 1: // cmd byte
+            currentTxByte = message->cmd;
+            break;
+        case 2: // size byte
+            currentTxByte = (_u8)message->getPayloadSize();
+            break;
+        default:
+        {
+            size_t payloadPos = currentPos - 3;
+            if (payloadPos == message->getPayloadSize()) {
+                // checksum byte
+                currentTxByte = checksum;
+                assert(currentPos + 1 == writeSize);
+            }
+            else {
+                // payload
+                currentTxByte = message->getDataBuf()[payloadPos];
+            }
+        }
+        }
+
+
+        checksum ^= currentTxByte;
+        buffer[currentPos++] = currentTxByte;
+    } while (0);
+
+    *size = currentPos;
+}
+
+void   RPLidarProtocolCodec::onDecodeReset() {
+    rp::hal::AutoLocker autolock(_op_locker);
+    // flush the pending data
+    _decodingMessage.cleanData();
+    // reset to initial state
+    _rx_pos = 0;
+    _working_states = STATUS_WAIT_SYNC1;
+}
+
+
+void RPLidarProtocolCodec::onDecodeData(const void* buffer, size_t size)
+{
+    rp::hal::AutoLocker autolock(_op_locker);
+
+    const _u8* data = reinterpret_cast<const _u8*>(buffer);
+    const _u8* dataEnd = data + size;
+
+
+    while (data != dataEnd) {
+        _u8 currentByte = *data;
+        ++data;
+
+        switch (_working_states & ((_u32)STATUS_LOOP_MODE_FLAG - 1)) {
+        case STATUS_WAIT_SYNC1:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE1) {
+                _working_states = STATUS_WAIT_SYNC2;
+            }
+            break;
+        case STATUS_WAIT_SYNC2:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE2) {
+                _working_states = STATUS_WAIT_SIZE_FLAG;
+                _rx_pos = 0; // init rx pos for recv size and flag
+            }
+            else {
+                // reset to the initial state
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_WAIT_SIZE_FLAG:
+        {
+            assert(sizeof(_decodingMessage.len) >= 4);
+            _u8* byteArr = reinterpret_cast<_u8*>(&_decodingMessage.len);
+            byteArr[_rx_pos++] = currentByte;
+
+            if (_rx_pos == 4) {
+                _working_states = STATUS_WAIT_TYPE;
+                _decodingMessage.len = le32_to_cpu(_decodingMessage.len);
+
+                // 30bit size + 2bit flag has been received
+                _u32 flagbits = (_u32)(_decodingMessage.len >> RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT);
+                if (flagbits & RPLIDAR_ANS_PKTFLAG_LOOP) {
+                    _working_states |= STATUS_LOOP_MODE_FLAG;
+                }
+                _decodingMessage.len = (_decodingMessage.len & RPLIDAR_ANS_HEADER_SIZE_MASK);
+                // alloc buffer
+                _decodingMessage.fillData(NULL, _decodingMessage.getPayloadSize());
+                _rx_pos = 0;
+            }
+        }
+        break;
+        case STATUS_WAIT_TYPE:
+            // save the type field as a cmd 
+            _decodingMessage.cmd = currentByte;
+
+            // recv payload...
+            _working_states = (_working_states & STATUS_LOOP_MODE_FLAG)
+                | STATUS_RECV_PAYLOAD;
+
+            if (!_decodingMessage.getPayloadSize()) {
+                // zero payload packet? 
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_RECV_PAYLOAD:
+            _decodingMessage.getDataBuf()[_rx_pos++] = currentByte;
+
+            if ((size_t)_rx_pos == _decodingMessage.getPayloadSize()) {
+                if (_working_states & STATUS_LOOP_MODE_FLAG) {
+                    // rewind to the payload recv status in loop mode
+                    _rx_pos = 0;
+                }
+                else {
+                    // reset the decoder
+                    _working_states = STATUS_WAIT_SYNC1;
+                }
+
+                IProtocolMessageListener* cachedLister = _listener;
+
+                autolock.forceUnlock(); //unlock the oplock to prevent deadlock
+
+
+                if (cachedLister) {
+                    cachedLister->onProtocolMessageDecoded(_decodingMessage);
+                }
+
+                _op_locker.lock(); // relock it
+            }
+            break;
+        }
+
+    }
+}
+
+
+
+
+}}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_lidarprotocol_codec.h b/LIDAR/sdk/src/sl_lidarprotocol_codec.h
new file mode 100644
index 0000000..1cc62f7
--- /dev/null
+++ b/LIDAR/sdk/src/sl_lidarprotocol_codec.h
@@ -0,0 +1,88 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 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_async_transceiver.h"
+
+namespace sl { namespace internal {
+
+
+class IProtocolMessageListener {
+public:
+    virtual void onProtocolMessageDecoded(const ProtocolMessage&) = 0;
+};
+
+
+class RPLidarProtocolCodec : public IAsyncProtocolCodec
+{
+public:
+
+    enum {
+        STATUS_WAIT_SYNC1 = 0x0,
+        STATUS_WAIT_SYNC2 = 0x1,
+        STATUS_WAIT_SIZE_FLAG = 0x2,
+        STATUS_WAIT_TYPE = 0x3,
+        STATUS_RECV_PAYLOAD = 0x4,
+        STATUS_LOOP_MODE_FLAG = 0x80000000,
+    };
+
+    RPLidarProtocolCodec();
+
+    void exitLoopMode();
+
+
+    virtual size_t estimateLength(message_autoptr_t& message);
+
+
+    virtual void onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size);
+
+    virtual void   onDecodeReset();
+    virtual void   onDecodeData(const void* buffer, size_t size);
+    
+    void setMessageListener(IProtocolMessageListener* l);
+
+protected:
+
+    IProtocolMessageListener* _listener;
+    ProtocolMessage          _decodingMessage;
+    rp::hal::Locker          _op_locker;
+                            
+    _u32                     _working_states;
+    int                      _rx_pos;
+};
+
+}}
+
+
+
diff --git a/LIDAR/sdk/src/sl_serial_channel.cpp b/LIDAR/sdk/src/sl_serial_channel.cpp
new file mode 100644
index 0000000..f7faf3c
--- /dev/null
+++ b/LIDAR/sdk/src/sl_serial_channel.cpp
@@ -0,0 +1,146 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class SerialPortChannel : public ISerialPortChannel
+    {
+    public:
+        SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx())
+        {
+            _device = device;
+            _baudrate = baudrate;
+        }
+
+        ~SerialPortChannel()
+        {
+            if (_rxtxSerial)
+                delete _rxtxSerial;
+        }
+
+        bool bind(const std::string& device, sl_s32 baudrate)
+        {
+            _closePending = false;
+            return _rxtxSerial->bind(device.c_str(), baudrate);
+        }
+
+        bool open()
+        {
+            if(!bind(_device, _baudrate))
+                return false;
+            return _rxtxSerial->open();
+        }
+
+        void close()
+        {
+            _closePending = true;
+            _rxtxSerial->cancelOperation();
+            _rxtxSerial->close();
+        }
+        void flush()
+        {
+            _rxtxSerial->flush(0);
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            _word_size_t result;
+            size_t size_holder;
+            size_hint = 0;
+
+            if (_closePending) return  RESULT_OPERATION_TIMEOUT;
+
+            if (!_rxtxSerial->isOpened()) {
+                return RESULT_OPERATION_FAIL;
+            }
+
+            result = _rxtxSerial->waitfordata(1, timeoutInMs, &size_holder);
+            size_hint = size_holder;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_DEV_ERR)
+                return RESULT_OPERATION_FAIL;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_TIMEOUT)
+                return RESULT_OPERATION_TIMEOUT;
+
+            return RESULT_OK;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (_closePending) return false;
+            return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK);
+        }
+
+        int write(const void* data, size_t size)
+        {
+           return _rxtxSerial->senddata((const sl_u8 * )data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size);
+            return (int)lenRec;
+        }
+
+        void clearReadCache()
+        {
+           
+        }
+
+        void setDTR(bool dtr)
+        {
+            dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR();
+        }
+
+        int getChannelType() {
+            return CHANNEL_TYPE_SERIALPORT;
+        }
+
+    private:
+        rp::hal::serial_rxtx  * _rxtxSerial;
+        bool _closePending;
+        std::string _device;
+        int _baudrate;
+
+    };
+
+    Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate)
+    {
+        return new  SerialPortChannel(device, baudrate);
+    }
+
+}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_tcp_channel.cpp b/LIDAR/sdk/src/sl_tcp_channel.cpp
new file mode 100644
index 0000000..8351c41
--- /dev/null
+++ b/LIDAR/sdk/src/sl_tcp_channel.cpp
@@ -0,0 +1,124 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class TcpChannel : public IChannel
+    {
+    public:
+        TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+        bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return IS_OK(_binded_socket->connect(_socket));
+            
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+        
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->send(data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            _binded_socket->recv(buffer, size, lenRec);
+            return (int)lenRec;
+        }
+
+        void clearReadCache() {}
+
+        void setStatus(_u32 flag){}
+
+        int getChannelType() {
+            return CHANNEL_TYPE_TCP;
+        }
+    private:
+        rp::net::StreamSocket * _binded_socket;
+        rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+    };
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port)
+    {
+        return new  TcpChannel(ip, port);
+    }
+}
\ No newline at end of file
diff --git a/LIDAR/sdk/src/sl_udp_channel.cpp b/LIDAR/sdk/src/sl_udp_channel.cpp
new file mode 100644
index 0000000..df0808d
--- /dev/null
+++ b/LIDAR/sdk/src/sl_udp_channel.cpp
@@ -0,0 +1,129 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+	class UdpChannel : public IChannel
+	{
+	public:
+		UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+		bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return SL_IS_OK(_binded_socket->setPairAddress(&_socket));         
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+            clearReadCache();
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->sendTo(nullptr, data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t actualGet;
+
+            u_result ans = _binded_socket->recvFrom(buffer, size, actualGet);
+            if (IS_FAIL(ans)) return 0;
+            return actualGet;
+        
+        }
+
+        void clearReadCache() {
+            _binded_socket->clearRxCache();
+        }
+
+        void setStatus(_u32 flag){}
+        
+        int getChannelType() {
+            return CHANNEL_TYPE_UDP;
+        }
+
+	private:
+		rp::net::DGramSocket * _binded_socket;
+		rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+	};
+
+    Result<IChannel*> createUdpChannel(const std::string& ip, int port)
+    {
+        return new  UdpChannel(ip, port);
+    }
+}
\ No newline at end of file