feature:support for conneting horizon

sdk_core/src/base/command_callback.h

@@ -34,18 +34,18 @@ namespace livox {
 class CommandCallback {
  public:
   virtual ~CommandCallback() {}
-  virtual void operator()(uint8_t handle, void *data) = 0;
+  virtual void operator()(livox_status status,uint8_t handle, void *data) = 0;
 };
 
 template <class T, class ResponseType>
 class MemberFunctionCallback : public CommandCallback {
  public:
-  typedef void (T::*MemFn)(uint8_t status, uint8_t handle, ResponseType *data);
+  typedef void (T::*MemFn)(livox_status status, uint8_t handle, ResponseType *data);
 
   MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (this_) {
-      (this_->*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess, handle, static_cast<ResponseType *>(data));
+      (this_->*func_)(status, handle, static_cast<ResponseType *>(data));
     }
   }
 
@@ -57,14 +57,16 @@ class MemberFunctionCallback : public CommandCallback {
 template <class T>
 class MemberFunctionCallback<T, uint8_t> : public CommandCallback {
  public:
-  typedef void (T::*MemFn)(uint8_t status, uint8_t handle, uint8_t response);
+  typedef void (T::*MemFn)(livox_status status, uint8_t handle, uint8_t response);
 
   MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (this_) {
-      (this_->*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess,
-                      handle,
-                      static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
+      if (data == NULL) {
+        (this_->*func_)(status, handle, 0);
+      } else {
+        (this_->*func_)(status, handle, static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
+      }
     }
   }
 
@@ -83,13 +85,13 @@ boost::shared_ptr<CommandCallback> MakeCommandCallback(T *cls,
 template <class T>
 class FunctionStatusCallback : public CommandCallback {
  public:
-  typedef void (*Fn)(uint8_t status, uint8_t handle, T *data, void *client_data);
+  typedef void (*Fn)(livox_status status, uint8_t handle, T *data, void *client_data);
 
  public:
   FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (func_) {
-      (*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess, handle, static_cast<T *>(data), client_data_);
+      (*func_)(status, handle, static_cast<T *>(data), client_data_);
     }
   }
 
@@ -101,16 +103,16 @@ class FunctionStatusCallback : public CommandCallback {
 template <>
 class FunctionStatusCallback<uint8_t> : public CommandCallback {
  public:
-  typedef void (*Fn)(uint8_t status, uint8_t handle, uint8_t data, void *client_data);
+  typedef void (*Fn)(livox_status status, uint8_t handle, uint8_t data, void *client_data);
 
  public:
   FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (func_) {
       if (data == NULL) {
-        (*func_)(kStatusTimeout, handle, 0, client_data_);
+        (*func_)(status, handle, 0, client_data_);
       } else {
-        (*func_)(kStatusSuccess, handle, *(uint8_t *)data, client_data_);
+        (*func_)(status, handle, *(uint8_t *)data, client_data_);
       }
     }
   }
@@ -129,13 +131,13 @@ boost::shared_ptr<CommandCallback> MakeCommandCallback(typename FunctionStatusCa
 template <class T>
 class MessageCallback : public CommandCallback {
  public:
-  typedef void (*Fn)(uint8_t handle, T *data);
+  typedef void (*Fn)(livox_status status, uint8_t handle, T *data);
 
  public:
   MessageCallback(const Fn &func) : func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (func_) {
-      (*func_)(handle, static_cast<T *>(data));
+      (*func_)(status, handle, static_cast<T *>(data));
     }
   }
 
@@ -146,13 +148,13 @@ class MessageCallback : public CommandCallback {
 template <class ResponseType>
 class BoostFunctionMessageCallback : public CommandCallback {
  public:
-  typedef boost::function<void(uint8_t, ResponseType *)> Fn;
+  typedef boost::function<void(livox_status, uint8_t, ResponseType *)> Fn;
 
  public:
   BoostFunctionMessageCallback(const Fn &func) : func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status, uint8_t handle, void *data) {
     if (func_) {
-      func_(handle, static_cast<ResponseType *>(data));
+      func_(status, handle, static_cast<ResponseType *>(data));
     }
   }
 
@@ -175,12 +177,12 @@ boost::shared_ptr<CommandCallback> MakeMessageCallback(typename BoostFunctionMes
 template <class T, class ResponseType>
 class MemberMessageCallback : public CommandCallback {
  public:
-  typedef void (T::*MemFn)(uint8_t handle, ResponseType *data);
+  typedef void (T::*MemFn)(livox_status status, uint8_t handle, ResponseType *data);
 
   MemberMessageCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status,uint8_t handle, void *data) {
     if (this_) {
-      (this_->*func_)(handle, (ResponseType *)data);
+      (this_->*func_)(status, handle, (ResponseType *)data);
     }
   }
 
@@ -202,12 +204,12 @@ class BoostFunctionCallback : public CommandCallback {
  public:
   typedef boost::function<void(uint8_t, uint8_t, ResponseType *)> Fn;
   BoostFunctionCallback(const Fn &func) : func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status,uint8_t handle, void *data) {
     if (func_) {
       if (data == NULL) {
-        func_(kStatusTimeout, handle, NULL);
+        func_(status, handle, NULL);
       } else {
-        func_(kStatusSuccess, handle, (ResponseType *)data);
+        func_(status, handle, (ResponseType *)data);
       }
     }
   }
@@ -221,12 +223,12 @@ class BoostFunctionCallback<uint8_t> : public CommandCallback {
  public:
   typedef boost::function<void(uint8_t, uint8_t, uint8_t)> Fn;
   BoostFunctionCallback(const Fn &func) : func_(func) {}
-  void operator()(uint8_t handle, void *data) {
+  void operator()(livox_status status,uint8_t handle, void *data) {
     if (func_) {
       if (data == NULL) {
-        func_(kStatusTimeout, handle, 0);
+        func_(status, handle, 0);
       } else {
-        func_(kStatusSuccess, handle, static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
+        func_(status, handle, static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
       }
     }
   }

sdk_core/src/base/io_loop.cpp

@@ -39,6 +39,7 @@ bool IOLoop::Init() {
   if (mem_pool_ == NULL) {
     return false;
   }
+
 #ifdef WIN32
   apr_status_t rv =
       apr_pollset_create(&pollset_, kMaxPollCount, mem_pool_, APR_POLLSET_WAKEABLE);
@@ -80,6 +81,12 @@ void IOLoop::RemoveDelegate(apr_socket_t *sock, IOLoopDelegate *) {
   PostTask(boost::bind(&IOLoop::RemoveDelegateAsync, this, sock));
 }
 
+void IOLoop::RemoveDelegateSync(apr_socket_t *sock) {
+  apr_os_thread_t thread_id = apr_os_thread_current();
+  assert(apr_os_thread_equal(this->thread_id_, thread_id));
+  RemoveDelegateAsync(sock);
+}
+
 void IOLoop::Loop() {
   apr_int32_t num = 0;
   const apr_pollfd_t *ret_pfd = NULL;
@@ -88,7 +95,6 @@ void IOLoop::Loop() {
   if (rv == APR_SUCCESS) {
     for (int i = 0; i < num; i++) {
       ClientData *data = static_cast<ClientData *>(ret_pfd[i].client_data);
-      ;
       if (data) {
         IOLoopDelegate *delegate = data->first;
         if (delegate) {

sdk_core/src/base/io_loop.h

@@ -60,9 +60,12 @@ class IOLoop : public noncopyable {
   void Uninit();
   void AddDelegate(apr_socket_t *sock, IOLoopDelegate *delegate, void *data = NULL);
   void RemoveDelegate(apr_socket_t *sock, IOLoopDelegate *delegate);
+  void RemoveDelegateSync(apr_socket_t *sock);
   void Loop();
   bool Wakeup();
   void PostTask(const IOLoopTask &task);
+  void SetThreadId (apr_os_thread_t thread_id) { thread_id_ = thread_id; }
+  apr_os_thread_t GetThreadId () { return thread_id_; }
 
  private:
   void AddDelegateAsync(apr_socket_t *sock, IOLoopDelegate *delegate, void *data);
@@ -80,6 +83,7 @@ class IOLoop : public noncopyable {
   bool enable_timer_;
   bool enable_wake_;
   std::vector<IOLoopTask> pending_tasks_;
+  apr_os_thread_t thread_id_;
 };
 
 }  // namespace livox

sdk_core/src/base/io_thread.cpp

@@ -35,6 +35,9 @@ void IOThread::ThreadFunc() {
   if (loop_ == NULL) {
     return;
   }
+  apr_os_thread_t thread_id = apr_os_thread_current();
+  loop_->SetThreadId(thread_id);
+
   while (!IsQuit()) {
     loop_->Loop();
   }

sdk_core/src/base/network_util.cpp

@@ -26,9 +26,9 @@
 #ifdef WIN32
 #include <boost/scoped_array.hpp>
 #include <Winsock2.h>
-#include <Iphlpapi.h>
+#include <iphlpapi.h>
 #include <string>
-#pragma comment(lib,"Iphlpapi.lib")
+#pragma comment(lib,"iphlpapi.lib")
 #pragma comment(lib,"ws2_32.lib")
 #else
 #include <ifaddrs.h>
@@ -36,11 +36,13 @@
 
 namespace livox {
 namespace util {
-apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool nonblock) {
+apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool reuse_port, bool nonblock) {
   apr_sockaddr_t *sa = NULL;
   apr_socket_t *s = NULL;
   apr_status_t rv = APR_SUCCESS;
 
+
+
   rv = apr_sockaddr_info_get(&sa, NULL, APR_INET, port, 0, mem_pool);
   if (rv != APR_SUCCESS) {
     return NULL;
@@ -51,6 +53,15 @@ apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool nonbloc
     return NULL;
   }
 
+  if (reuse_port) {
+    rv = apr_socket_opt_set(s, APR_SO_REUSEADDR, 1);
+    if (rv != APR_SUCCESS) {
+      apr_socket_close(s);
+      s = NULL;
+      return NULL;
+    }
+  }
+
   rv = apr_socket_bind(s, sa);
   if (rv != APR_SUCCESS) {
     apr_socket_close(s);
@@ -87,7 +98,7 @@ bool GetAdapterState(const IP_ADAPTER_INFO *pAdapter)
   return true;
 }
 
-bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
+bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
   bool found = false;
   ULONG ulOutbufLen = sizeof(IP_ADAPTER_INFO);
   boost::scoped_array<uint8_t> pAdapterInfo(new uint8_t[ulOutbufLen]);
@@ -99,29 +110,25 @@ bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
 
   IP_ADAPTER_INFO *pAdapter = reinterpret_cast<IP_ADAPTER_INFO *>(pAdapterInfo.get());
   if (NO_ERROR == dlRetVal && pAdapter != NULL) {
-    while (pAdapter != NULL) {
-      if (GetAdapterState(pAdapter)) {
-	std::string str_ip = pAdapter->IpAddressList.IpAddress.String;
-	std::string str_mask = pAdapter->IpAddressList.IpMask.String;
-	ULONG host_ip = inet_addr(const_cast<char *>(str_ip.c_str()));
-	ULONG host_mask = inet_addr(const_cast<char *>(str_mask.c_str()));
+    while (GetAdapterState(pAdapter)) {
+      std::string str_ip = pAdapter->IpAddressList.IpAddress.String;
+      std::string str_mask = pAdapter->IpAddressList.IpMask.String;
+      ULONG host_ip = inet_addr(const_cast<char *>(str_ip.c_str()));
+      ULONG host_mask = inet_addr(const_cast<char *>(str_mask.c_str()));
 
-	if ((host_ip & host_mask) ==
-	  (client_addr.sin_addr.S_un.S_addr & host_mask)) {
-	  local_ip = host_ip;
-	  found = true;
-	  break;
-	}
+      if ((host_ip & host_mask) ==
+        (client_addr.sin_addr.S_un.S_addr & host_mask)) {
+        local_ip = host_ip;
+        found = true;
+        break;
       }
-
       pAdapter = pAdapter->Next;
     }
   }
-
   return found;
 }
 #else
-bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
+bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
   struct ifaddrs *if_addrs = NULL, *addrs = NULL;
   if (getifaddrs(&if_addrs) == -1) {
     return false;
@@ -153,4 +160,4 @@ bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
 }
 #endif
 }  // namespace util
-}  // namespace livox
+}  // namespace livox

sdk_core/src/base/network_util.h

@@ -34,8 +34,8 @@
 namespace livox {
 namespace util {
 
-apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool nonblock = true);
-bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip);
+apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool reuse_port = false, bool nonblock = true);
+bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip);
 
 }  // namespace util
 }  // namespace livox

sdk_core/src/base/thread_base.h

@@ -26,6 +26,7 @@
 #define LIVOX_THREAD_BASE_H_
 #include <apr_general.h>
 #include <apr_thread_proc.h>
+#include <apr_portable.h>
 #include <boost/atomic/atomic.hpp>
 #include "noncopyable.h"
 

sdk_core/src/command_handler/command_channel.cpp

@@ -149,7 +149,12 @@ void CommandChannel::OnTimer(apr_time_t now) {
 
 void CommandChannel::Uninit() {
   if (sock_) {
-    loop_->RemoveDelegate(sock_, this);
+    apr_os_thread_t thread_id = apr_os_thread_current();
+    if (apr_os_thread_equal(loop_->GetThreadId(), thread_id)) {
+      loop_->RemoveDelegateSync(sock_);
+    } else {
+      loop_->RemoveDelegate(sock_, this);
+    }
     loop_ = NULL;
     apr_socket_close(sock_);
     sock_ = NULL;
@@ -195,6 +200,9 @@ void CommandChannel::SendInternal(const Command &command) {
   if (rv == APR_SUCCESS) {
     rv = apr_socket_sendto(sock_, sa, 0, (const char *)buf, &apr_size);
   }
+  if (rv != APR_SUCCESS) {
+    (*command.cb)(kStatusSendFailed, handle_, NULL);
+  }
   apr_pool_destroy(subpool);
 }
 
@@ -226,7 +234,7 @@ void CommandChannel::OnHeartbeatAck(const CommPacket &) {
 }
 
 void CommandChannel::DeviceDisconnect(uint8_t handle) {
-  DeviceRemove(handle);
+  DeviceRemove(handle,kEventDisconnect);
 }
 
 void CommandChannel::Send(const Command &command) {

sdk_core/src/command_handler/command_handler.cpp

@@ -61,6 +61,32 @@ uint16_t GetCommandTimeout(uint8_t command_set, uint8_t command_id) {
   return KDefaultTimeOut;
 }
 
+bool IsSubLidarException(const Command &command) {
+  if (device_manager().device_mode() == kDeviceModeLidar) {
+    return false;
+  }
+  if (!(command.packet.cmd_set == kCommandSetGeneral &&
+      command.packet.cmd_code == kCommandIDGeneralPushAbnormalState)) {
+    return false;
+  }
+
+  HubErrorCode* hub_error_code = static_cast<HubErrorCode *>((void *)command.packet.data);
+  if (hub_error_code->lidar_link_status == 0x01) {
+    return true;
+  }
+  return false;
+}
+
+bool IsMidDeviceIpResponse(const Command &command) {
+  return command.packet.cmd_set == kCommandSetGeneral &&
+         command.packet.cmd_code == kCommandIDGeneralGetDeviceIpInformation &&
+         command.packet.data_len != sizeof(GetDeviceIpModeResponse);
+}
+
+void OnSubLidarDisconnect() {
+  device_manager().UpdateDevices(DeviceInfo(), kEventHubConnectionChange);
+}
+
 bool CommandHandler::AddDevice(const DeviceInfo &info) {
   if (impl_ == NULL) {
     DeviceMode mode = static_cast<DeviceMode>(device_manager().device_mode());
@@ -109,14 +135,14 @@ void CommandHandler::OnCommand(uint8_t handle, const Command &command) {
   }
 }
 
-bool CommandHandler::SendCommand(uint8_t handle,
-                                 uint8_t command_set,
-                                 uint8_t command_id,
-                                 uint8_t *data,
-                                 uint16_t length,
-                                 const shared_ptr<CommandCallback> &cb) {
+livox_status CommandHandler::SendCommand(uint8_t handle,
+                                         uint8_t command_set,
+                                         uint8_t command_id,
+                                         uint8_t *data,
+                                         uint16_t length,
+                                         const shared_ptr<CommandCallback> &cb) {
   if (impl_ == NULL) {
-    return false;
+    return kStatusHandlerImplNotExist;
   }
 
   Command cmd(handle,
@@ -128,24 +154,41 @@ bool CommandHandler::SendCommand(uint8_t handle,
               length,
               GetCommandTimeout(command_set, command_id),
               cb);
-  bool result = impl_->SendCommand(handle, cmd);
+  livox_status result = impl_->SendCommand(handle, cmd);
   return result;
 }
 
-bool CommandHandler::RegisterPush(uint8_t handle,
-                                  uint8_t command_set,
-                                  uint8_t command_id,
-                                  const shared_ptr<CommandCallback> &cb) {
+livox_status CommandHandler::RegisterPush(uint8_t handle,
+                                          uint8_t command_set,
+                                          uint8_t command_id,
+                                          const shared_ptr<CommandCallback> &cb) {
   lock_guard<mutex> lock(mutex_);
   Command req(handle, kCommandTypeMsg, command_set, command_id, 0, NULL, 0, 0, cb);
   message_registers_.insert(make_pair(MakeKey(command_set, command_id), req));
-  return true;
+  return kStatusSuccess;
 }
 
 void CommandHandler::OnCommandAck(uint8_t handle, const Command &command) {
-  if (command.cb != NULL) {
-    (*command.cb)(handle, command.packet.data);
+  if (command.cb == NULL) {
+    return;
   }
+  if (command.packet.data == NULL) {
+    (*command.cb)(kStatusTimeout, handle, command.packet.data);
+    return;
+  }
+
+  if (IsMidDeviceIpResponse(command)) {
+    GetDeviceIpModeResponse  response;
+    memcpy(&response, command.packet.data, command.packet.data_len);
+    uint8_t net_mask[4] = {255, 255, 255, 0};
+    memcpy(&response.net_mask, net_mask, 4);
+    uint8_t gw_addr[4] = {192, 168, 1, 1};
+    memcpy(&response.gw_addr, gw_addr, 4);
+
+    (*command.cb)(kStatusSuccess, handle, (uint8_t *)&response);
+    return;
+  }
+  (*command.cb)(kStatusSuccess, handle, command.packet.data);
 }
 
 void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) {
@@ -161,10 +204,13 @@ void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) {
       }
     }
   }
+  if (IsSubLidarException(command)) {
+    OnSubLidarDisconnect();
+  }
 
   for (list<Command>::iterator ite = commands.begin(); ite != commands.end(); ++ite) {
     if (ite->cb) {
-      (*ite->cb)(handle, command.packet.data);
+      (*ite->cb)(kStatusSuccess, handle, command.packet.data);
     }
   }
 }

sdk_core/src/command_handler/command_handler.h

@@ -46,17 +46,17 @@ class CommandHandler : public noncopyable {
   bool AddDevice(const DeviceInfo &info);
   void RemoveDevice(uint8_t handle);
 
-  bool SendCommand(uint8_t handle,
-                   uint8_t command_set,
-                   uint8_t command_id,
-                   uint8_t *data,
-                   uint16_t length,
-                   const boost::shared_ptr<CommandCallback> &cb);
+  livox_status SendCommand(uint8_t handle,
+                           uint8_t command_set,
+                           uint8_t command_id,
+                           uint8_t *data,
+                           uint16_t length,
+                           const boost::shared_ptr<CommandCallback> &cb);
 
-  bool RegisterPush(uint8_t handle,
-                    uint8_t command_set,
-                    uint8_t command_id,
-                    const boost::shared_ptr<CommandCallback> &cb);
+  livox_status RegisterPush(uint8_t handle,
+                            uint8_t command_set,
+                            uint8_t command_id,
+                            const boost::shared_ptr<CommandCallback> &cb);
 
   void OnCommand(uint8_t handle, const Command &command);
 
@@ -84,7 +84,7 @@ class CommandHandlerImpl : public CommandChannelDelegate {
   virtual bool AddDevice(const DeviceInfo &info) = 0;
   virtual bool RemoveDevice(uint8_t handle) = 0;
 
-  virtual bool SendCommand(uint8_t handle, const Command &command) = 0;
+  virtual livox_status SendCommand(uint8_t handle, const Command &command) = 0;
 
   virtual void OnCommand(uint8_t handle, const Command &command) {
     if (handler_) {

sdk_core/src/command_handler/command_impl.cpp

@@ -31,6 +31,9 @@
 #include "livox_def.h"
 #include "livox_sdk.h"
 
+# define HMS_PARSE_BUF (128)
+# define YY_PARSE_BUF (128)
+
 using std::pair;
 using std::vector;
 using namespace livox;
@@ -43,7 +46,7 @@ void SetBroadcastCallback(DeviceBroadcastCallback cb) {
   device_manager().SetDeviceBroadcastCallback(cb);
 }
 
-uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
+livox_status AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
   bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeHub, *handle);
   if (result) {
     return kStatusSuccess;
@@ -52,7 +55,7 @@ uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
   }
 }
 
-uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
+livox_status AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
   bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeLidar, *handle);
   if (result) {
     return kStatusSuccess;
@@ -61,7 +64,7 @@ uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
   }
 }
 
-uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
+livox_status GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
   if (devices == NULL || size == NULL) {
     return kStatusFailure;
   }
@@ -79,284 +82,668 @@ void SetDataCallback(uint8_t handle, DataCallback cb, void *client_data) {
   data_handler().AddDataListener(handle, cb, client_data);
 }
 
-uint8_t DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
+livox_status DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
   uint8_t req = enable;
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralControlSample,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralControlSample,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
+livox_status LidarFanControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  uint8_t req = static_cast<uint8_t>(enable);
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarControlFan,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+livox_status SetDeviceIp(uint8_t handle, SetDeviceIpExtendModeRequest *req, CommonCommandCallback cb, void *client_data) {
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralConfigureStaticDynamicIp,
+                                                      (uint8_t*)req,
+                                                      sizeof(*req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+bool ChecksumRmc(const char* buff_begin, const char* buff_end) {
+  //checksum $GPRMC/$GNRMC format : *hh
+  const uint8_t head_size = strlen("$");
+  const uint8_t tail_size = strlen("hh");
+  char tail[3] = { 0 };
+  uint32_t tail_num = 0;
+  uint8_t crc = 0;
+  buff_begin += head_size;
+
+  for ( ; buff_begin < buff_end; buff_begin++) {
+    if (*buff_begin == '*') {
+      if (buff_begin + tail_size <= buff_end) {
+        strncpy(tail, &buff_begin[1], tail_size);
+        sscanf(tail, "%x", &tail_num);
+        crc ^= (uint8_t)tail_num;
+        if (crc == 0) {
+          return true;
+        }
+        break;
+      }
+    }
+    crc ^= *buff_begin;
+  }
+  return false;
+}
+
+bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest* utc_time_req) {
+  const char* rmc_begin = strstr(rmc, "$GPRMC");
+  if (rmc_begin == NULL) {
+    rmc_begin = strstr(rmc, "$GNRMC");
+  }
+  if (rmc_begin == NULL) {
+    return false;
+  }
+  if (!ChecksumRmc(rmc_begin, rmc + rmc_len)) {
+    return false;
+  }
+
+  char utc_hms_buff[HMS_PARSE_BUF] = { 0 };
+  char utc_yy_buff[YY_PARSE_BUF] = { 0 };
+  int hour = 0, minute = 0, second = 0, millisecond = 0;
+
+  memset(utc_time_req, 0, sizeof(LidarSetUtcSyncTimeRequest));
+
+  if (4 != sscanf(rmc_begin,
+                  "$G%*[NP]RMC,%[^,],%*C,%*f,%*C,%*f,%*C,%*f,%*f,%2hhu%2hhu%[^,],%*f,",
+                  &utc_hms_buff[0],
+                  &(utc_time_req->day),
+                  &(utc_time_req->month),
+                  &utc_yy_buff[0])) {
+    return false;
+  }
+
+  int time_buff_size = strlen(utc_yy_buff);
+  uint8_t temp_time = 0;
+  switch (time_buff_size) {
+    case sizeof("yyyy")-1:
+      if (strncmp(utc_yy_buff, "2000", strlen("2000")) < 0) {
+        return false;
+      }
+      if (1 != sscanf(utc_yy_buff, "%*C%*C%2hhu", &(utc_time_req->year))) {
+        return false;
+      }
+      temp_time = utc_time_req->day;
+      utc_time_req->day = utc_time_req->month;
+      utc_time_req->month = temp_time;
+      break;
+    case sizeof("yy")-1:
+      if (1 != sscanf(utc_yy_buff, "%2hhu", &(utc_time_req->year))) {
+        return false;
+      }
+      break;
+    default:
+      return false;
+  }
+
+  time_buff_size = strlen(utc_hms_buff);
+  switch (time_buff_size) {
+    case sizeof("hhmmss")-1:
+      if (3 != sscanf(utc_hms_buff, "%2d%2d%2d", &hour, &minute, &second)) {
+        return false;
+      }
+      break;
+    case sizeof("hhmmss.s")-1:
+    case sizeof("hhmmss.ss")-1:
+    case sizeof("hhmmss.sss")-1:
+      if (4 != sscanf(utc_hms_buff, "%2d%2d%2d.%2d", &hour, &minute, &second, &millisecond)) {
+        return false;
+      }
+      break;
+    default:
+      return false;
+  }
+  utc_time_req->mircrosecond = (minute * 60 * 1000 + second * 1000 + millisecond) * 1000;
+  return true;
+}
+
+livox_status LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeLidar) {
     return kStatusNotSupported;
   }
-  return DeviceSampleControl(handle, true, cb, data);
+  return DeviceSampleControl(handle, true, cb, client_data);
 }
 
-uint8_t LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
+livox_status LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeLidar) {
     return kStatusNotSupported;
   }
-  return DeviceSampleControl(handle, false, cb, data);
+  return DeviceSampleControl(handle, false, cb, client_data);
 }
 
-uint8_t HubStartSampling(CommonCommandCallback cb, void *client_data) {
+livox_status HubStartSampling(CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
   return DeviceSampleControl(kHubDefaultHandle, true, cb, client_data);
 }
 
-uint8_t HubStopSampling(CommonCommandCallback cb, void *client_data) {
+livox_status HubStopSampling(CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
   return DeviceSampleControl(kHubDefaultHandle, false, cb, client_data);
 }
 
-uint8_t HubGetLidarHandle(uint8_t slot, uint8_t id) {
+livox_status HubGetLidarHandle(uint8_t slot, uint8_t id) {
   return (slot - 1) * 3 + id - 1;
 }
 
-uint8_t QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *data) {
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralDeviceInfo,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<DeviceInformationResponse>(cb, data));
-  return result ? kStatusSuccess : kStatusFailure;
+livox_status QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *client_data) {
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralDeviceInfo,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<DeviceInformationResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+
+livox_status DisconnectDevice(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralDisconnect,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+livox_status SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
   uint8_t req = 0;
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralCoordinateSystem,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralCoordinateSystem,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+livox_status SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
   uint8_t req = 1;
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralCoordinateSystem,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralCoordinateSystem,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) {
-  bool result = command_handler().RegisterPush(
+livox_status SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) {
+  livox_status result = command_handler().RegisterPush(
       handle, kCommandSetGeneral, kCommandIDGeneralPushAbnormalState, MakeMessageCallback<ErrorMessage>(cb));
-  return result ? kStatusSuccess : kStatusFailure;
+  return result;
 }
 
-uint8_t SetStaticDynamicIP(uint8_t handle,
-                           SetDeviceIPModeRequest *req,
-                           CommonCommandCallback cb,
-                           void *client_data) {
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralConfigureStaticDynamicIP,
-                                              (uint8_t *)req,
-                                              sizeof(*req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+livox_status SetStaticDynamicIP(uint8_t handle,
+                                SetDeviceIPModeRequest *req,
+                                CommonCommandCallback cb,
+                                void *client_data) {
+  if(device_manager().device_mode() == kDeviceModeLidar
+     && !device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  SetDeviceIpExtendModeRequest ext_req;
+  ext_req.ip_mode = req->ip_mode;
+  ext_req.ip_addr = req->ip_addr;
+  uint8_t net_mask[4] = {255, 255, 255, 0};
+  memcpy(&ext_req.net_mask, net_mask, 4);
+  uint8_t gw_addr[4] = {192, 168, 1, 1};
+  memcpy(&ext_req.gw_addr, gw_addr, 4);
+  return SetDeviceIp(handle, &ext_req, cb, client_data);
 }
 
-uint8_t GetDeviceIPInformation(uint8_t handle, GetDeviceIPInformationCallback cb, void *client_data) {
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetGeneral,
-                                              kCommandIDGeneralGetDeviceIPInformation,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<GetDeviceIPModeResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+livox_status SetDynamicIp(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+  SetDeviceIpExtendModeRequest req;
+  req.ip_mode = kLidarDynamicIpMode;  
+  return SetDeviceIp(handle, &req, cb, client_data);
 }
 
-uint8_t LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) {
+livox_status SetStaticIp(uint8_t handle,
+                         SetStaticDeviceIpModeRequest *req,
+                         CommonCommandCallback cb,
+                         void *client_data) {
+  SetDeviceIpExtendModeRequest static_ip_req = { kLidarStaticIpMode,req->ip_addr,req->net_mask,req->gw_addr};
+  return SetDeviceIp(handle, &static_ip_req, cb, client_data);
+}
+
+livox_status GetDeviceIpInformation(uint8_t handle, GetDeviceIpInformationCallback cb, void *client_data) {
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralGetDeviceIpInformation,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<GetDeviceIpModeResponse>(cb, client_data));
+  return result;
+}
+
+livox_status RebootDevice(uint8_t handle, uint16_t timeout, CommonCommandCallback cb, void * client_data) {
+  if(device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetGeneral,
+                                                      kCommandIDGeneralRebootDevice,
+                                                      (uint8_t *)(&timeout),
+                                                      sizeof(timeout),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+livox_status LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeLidar) {
     return kStatusNotSupported;
   }
   uint8_t req = static_cast<uint8_t>(mode);
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetLidar,
-                                              kCommandIDLidarSetMode,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarSetMode,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t LidarSetExtrinsicParameter(uint8_t handle,
-                                   LidarSetExtrinsicParameterRequest *req,
-                                   CommonCommandCallback cb,
-                                   void *client_data) {
+livox_status LidarSetExtrinsicParameter(uint8_t handle,
+                                        LidarSetExtrinsicParameterRequest *req,
+                                        CommonCommandCallback cb,
+                                        void *client_data) {
   if (device_manager().device_mode() != kDeviceModeLidar) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetLidar,
-                                              kCommandIDLidarSetExtrinsicParameter,
-                                              (uint8_t *)req,
-                                              sizeof(*req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarSetExtrinsicParameter,
+                                                      (uint8_t *)req,
+                                                      sizeof(*req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data) {
+livox_status LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeLidar) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetLidar,
-                                              kCommandIDLidarGetExtrinsicParameter,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<LidarGetExtrinsicParameterResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarGetExtrinsicParameter,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<LidarGetExtrinsicParameterResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
+livox_status LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar 
+      || !device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
   uint8_t req = static_cast<uint8_t>(enable);
-  bool result = command_handler().SendCommand(handle,
-                                              kCommandSetLidar,
-                                              kCommandIDLidarControlRainFogSuppression,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarControlRainFogSuppression,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data) {
+livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  return LidarFanControl(handle, true, cb, client_data);
+}
+
+livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  return LidarFanControl(handle, false, cb, client_data);
+}
+
+livox_status LidarGetFanState(uint8_t handle, LidarGetFanStateCallback cb, void * data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarGetFanState,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<LidarGetFanStateResponse>(cb, data));
+  return result;
+}
+
+livox_status LidarSetPointCloudReturnMode(uint8_t handle, PointCloudReturnMode mode,  CommonCommandCallback cb, void * data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  uint8_t req = static_cast<uint8_t>(mode);
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarSetPointCloudReturnMode,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, data));
+   return result;
+}
+
+livox_status LidarGetPointCloudReturnMode(uint8_t handle, LidarGetPointCloudReturnModeCallback cb, void * client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarGetPointCloudReturnMode,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<LidarGetPointCloudReturnModeResponse>(cb, client_data));
+   return result;
+}
+
+livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonCommandCallback cb, void * client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  uint8_t req = static_cast<uint8_t>(freq);
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarSetImuPushFrequency,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+livox_status LidarGetImuPushFrequency(uint8_t handle, LidarGetImuPushFrequencyCallback cb, void * data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarGetImuPushFrequency,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<LidarGetImuPushFrequencyResponse>(cb, data));
+  return result;
+}
+
+livox_status HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubQueryLidarInformation,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<HubQueryLidarInformationResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubQueryLidarInformation,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<HubQueryLidarInformationResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data) {
+livox_status HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubSetMode,
-                                              (uint8_t *)req,
-                                              length,
-                                              MakeCommandCallback<HubSetModeResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubSetMode,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubSetModeResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data) {
+livox_status HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubControlSlotPower,
-                                              (uint8_t *)req,
-                                              sizeof(*req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubControlSlotPower,
+                                                      (uint8_t *)req,
+                                                      sizeof(*req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
-                                 uint16_t length,
-                                 HubSetExtrinsicParameterCallback cb,
-                                 void *client_data) {
+livox_status HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
+                                      uint16_t length,
+                                      HubSetExtrinsicParameterCallback cb,
+                                      void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubSetExtrinsicParameter,
-                                              (uint8_t *)req,
-                                              length,
-                                              MakeCommandCallback<HubSetExtrinsicParameterResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubSetExtrinsicParameter,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubSetExtrinsicParameterResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterCallback cb, void *client_data) {
+livox_status HubGetExtrinsicParameter(HubGetExtrinsicParameterCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubGetExtrinsicParameter,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubGetExtrinsicParameter,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data) {
+livox_status HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubQueryLidarDeviceStatus,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<HubQueryLidarStatusResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubQueryLidarDeviceStatus,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<HubQueryLidarStatusResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) {
+livox_status HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
   uint8_t req = static_cast<uint8_t>(enable);
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubExtrinsicParameterCalculation,
-                                              &req,
-                                              sizeof(req),
-                                              MakeCommandCallback<uint8_t>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubExtrinsicParameterCalculation,
+                                                      &req,
+                                                      sizeof(req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
 }
 
-uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req,
-                           uint16_t length,
-                           HubRainFogSuppressCallback cb,
+livox_status HubRainFogSuppress(HubRainFogSuppressRequest *req,
+                                uint16_t length,
+                                HubRainFogSuppressCallback cb,
+                                void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubRainFogSuppression,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubQuerySlotPowerStatus(HubQuerySlotPowerStatusCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubQuerySlotPowerStatus,
+                                                      NULL,
+                                                      0,
+                                                      MakeCommandCallback<HubQuerySlotPowerStatusResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubFanControl(HubFanControlRequest *req, 
+                           uint16_t length, 
+                           HubFanControlCallback cb, 
                            void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubRainFogSuppression,
-                                              (uint8_t *)req,
-                                              length,
-                                              MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubControlFan,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubFanControlResponse>(cb, client_data));
+  return result;
 }
 
-uint8_t HubQuerySlotPowerStatus(HubQuerySlotPowerStatusCallback cb, void *client_data) {
+livox_status HubGetFanState(HubGetFanStateRequest *req, 
+                            uint16_t length, 
+                            HubGetFanStateCallback cb, 
+                            void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
-  bool result = command_handler().SendCommand(kHubDefaultHandle,
-                                              kCommandSetHub,
-                                              kCommandIDHubQuerySlotPowerStatus,
-                                              NULL,
-                                              0,
-                                              MakeCommandCallback<HubQuerySlotPowerStatusResponse>(cb, client_data));
-  return result ? kStatusSuccess : kStatusFailure;
-}
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubGetFanState,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubGetFanStateResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubSetPointCloudReturnMode(HubSetPointCloudReturnModeRequest *req, 
+                                        uint16_t length, 
+                                        HubSetPointCloudReturnModeCallback cb, 
+                                        void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubSetPointCloudReturnMode,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubSetPointCloudReturnModeResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubGetPointCloudReturnMode(HubGetPointCloudReturnModeRequest *req, 
+                                        uint16_t length, 
+                                        HubGetPointCloudReturnModeCallback cb, 
+                                        void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubGetPointCloudReturnMode,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubGetPointCloudReturnModeResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubSetImuPushFrequency(HubSetImuPushFrequencyRequest *req, 
+                                    uint16_t length, 
+                                    HubSetImuPushFrequencyCallback cb, 
+                                    void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubSetImuPushFrequency,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubSetImuPushFrequencyResponse>(cb, client_data));
+  return result;
+}
+
+livox_status HubGetImuPushFrequency(HubGetImuPushFrequencyRequest *req, 
+                                    uint16_t length, 
+                                    HubGetImuPushFrequencyCallback cb, 
+                                    void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  livox_status result = command_handler().SendCommand(kHubDefaultHandle,
+                                                      kCommandSetHub,
+                                                      kCommandIDHubGetImuPushFrequency,
+                                                      (uint8_t *)req,
+                                                      length,
+                                                      MakeCommandCallback<HubGetImuPushFrequencyResponse>(cb, client_data));
+  return result;
+}
+
+livox_status LidarSetUtcSyncTime(uint8_t handle,
+                                 LidarSetUtcSyncTimeRequest* req,
+                                 CommonCommandCallback cb ,
+                                 void *client_data) {
+  livox_status result = command_handler().SendCommand(handle,
+                                                      kCommandSetLidar,
+                                                      kCommandIDLidarSetSyncTime,
+                                                      (uint8_t *)req,
+                                                      sizeof(*req),
+                                                      MakeCommandCallback<uint8_t>(cb, client_data));
+  return result;
+}
+
+livox_status LidarSetRmcSyncTime(uint8_t handle,
+                                 const char* rmc,
+                                 uint16_t rmc_length,
+                                 CommonCommandCallback cb,
+                                 void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeLidar
+      || device_manager().IsLidarMid40(handle)) {
+    return kStatusNotSupported;
+  }
+ 
+  LidarSetUtcSyncTimeRequest utc_time_req;
+  if (!ParseRmcTime(rmc, rmc_length, &utc_time_req)) {
+    return kStatusFailure;
+  }
+
+  return LidarSetUtcSyncTime(handle, &utc_time_req, cb, client_data);
+}

sdk_core/src/command_handler/command_impl.h

@@ -75,11 +75,15 @@ typedef enum {
   /**
    * General command set, set the IP of the a device.
    */
-  kCommandIDGeneralConfigureStaticDynamicIP = 8,
+  kCommandIDGeneralConfigureStaticDynamicIp = 8,
   /**
    * General command set, get the IP of the a device.
    */
-  kCommandIDGeneralGetDeviceIPInformation = 9,
+  kCommandIDGeneralGetDeviceIpInformation = 9,
+  /**
+   * General command set, reboot a device.
+   */
+  kCommandIDGeneralRebootDevice = 0x0a,
 
   /**
    * ******************************************************
@@ -97,6 +101,7 @@ static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KD
                                                                               KDefaultTimeOut,
                                                                               KDefaultTimeOut,
                                                                               KDefaultTimeOut,
+                                                                              KDefaultTimeOut,
                                                                               KDefaultTimeOut};
 
 /**  Enum that represents the command id. */
@@ -117,7 +122,34 @@ typedef enum {
    * Lidar command set, enable or disable the rain/fog suppression of a LiDAR.
    */
   kCommandIDLidarControlRainFogSuppression = 3,
-
+  /**
+   * Lidar command set, turn on\off fan of a LiDAR.
+   */
+  kCommandIDLidarControlFan = 4,
+    /**
+   * Lidar command set, get fan state of a LiDAR.
+   */
+  kCommandIDLidarGetFanState = 5,
+  /**
+   * Lidar command set, set point cloud return mode of a LiDAR.
+   */
+  kCommandIDLidarSetPointCloudReturnMode = 6,
+  /**
+   * Lidar command set, get point cloud return mode of a LiDAR.
+   */
+  kCommandIDLidarGetPointCloudReturnMode = 7,
+  /**
+   * Lidar command set, set IMU push frequency of a LiDAR.
+   */
+  kCommandIDLidarSetImuPushFrequency = 8,
+  /**
+   * Lidar command set, get IMU push frequency of a LiDAR.
+   */
+  kCommandIDLidarGetImuPushFrequency = 9,
+  /**
+   * Lidar command set, set synchronization time of a LiDAR.
+   */
+  kCommandIDLidarSetSyncTime = 0x0a,
   /**
    * ******************************************************
    * Don't add command id after kCommandIDLidarCommandCount.
@@ -126,6 +158,13 @@ typedef enum {
 } LidarCommandID;
 
 static const uint16_t LidarCommandTimeout[kCommandIDLidarCommandCount] = {KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
+                                                                          KDefaultTimeOut,
                                                                           KDefaultTimeOut,
                                                                           KDefaultTimeOut,
                                                                           KDefaultTimeOut};
@@ -168,6 +207,30 @@ typedef enum {
   * Hub command set, get the power supply state of each hub slot.
   */
   kCommandIDHubQuerySlotPowerStatus = 8,
+  /**
+  * Hub command set, turn on\off fan of the connected Livox LiDAR.
+  */
+  kCommandIDHubControlFan = 9,
+  /**
+  * Hub command set, get the fan state of the connected Livox LiDAR.
+  */
+  kCommandIDHubGetFanState = 0x0a,
+  /**
+   * Hub command set, set point cloud return mode of the connected Livox LiDAR.
+   */
+  kCommandIDHubSetPointCloudReturnMode = 0x0b,
+  /**
+  * Hub command set, get point cloud return mode of the connected Livox LiDAR.
+  */
+  kCommandIDHubGetPointCloudReturnMode = 0x0c,
+  /**
+  * Hub command set, set IMU push frequency of the connected Livox LiDAR.
+  */
+  kCommandIDHubSetImuPushFrequency = 0x0d,
+  /**
+  * Hub command set, get IMU push frequency of the connected Livox LiDAR.
+  */
+  kCommandIDHubGetImuPushFrequency = 0x0e,
 
   /**
    * ******************************************************
@@ -177,6 +240,12 @@ typedef enum {
 } HubCommandID;
 
 static const uint16_t HubCommandTimeout[kCommandIDHubCommandCount] = {KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
                                                                       KDefaultTimeOut,
                                                                       KDefaultTimeOut,
                                                                       KDefaultTimeOut,
@@ -197,5 +266,18 @@ typedef enum {
   /** message type, which is sent at a specified frequency. */
   kCommandTypeMsg = 2
 } CommandType;
+
+#pragma pack(1)
+/**
+ * The request body of the command for setting device's IP mode.
+ */
+typedef struct {
+  uint8_t ip_mode;    /**< IP address mode: 0 for dynamic IP address, 1 for static IP address. */
+  uint32_t ip_addr;   /**< IP address. */
+  uint32_t net_mask;  /**< Subnet mask. */
+  uint32_t gw_addr;   /**< Gateway address. */
+} SetDeviceIpExtendModeRequest;
+#pragma pack()
+
 }  // namespace livox
 #endif  // LIVOX_SDK_COMMAND_IMPL_H

sdk_core/src/command_handler/hub_command_handler.cpp

@@ -46,12 +46,12 @@ bool HubCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
   return true;
 }
 
-bool HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) {
+livox_status HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) {
   if (channel_ == NULL) {
-    return false;
+    return kStatusChannelNotExist;
   }
   channel_->SendAsync(command);
-  return true;
+  return kStatusSuccess;
 }
 
 bool HubCommandHandlerImpl::RemoveDevice(uint8_t) {

sdk_core/src/command_handler/hub_command_handler.h

@@ -36,7 +36,7 @@ class HubCommandHandlerImpl : public CommandHandlerImpl {
   void Uninit();
   bool AddDevice(const DeviceInfo &info);
   bool RemoveDevice(uint8_t handle);
-  bool SendCommand(uint8_t handle, const Command &command);
+  livox_status SendCommand(uint8_t handle, const Command &command);
 
  private:
   apr_pool_t *mem_pool_;

sdk_core/src/command_handler/lidar_command_handler.cpp

@@ -42,20 +42,25 @@ bool LidarCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
   return true;
 }
 
-bool LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) {
+livox_status LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) {
   CommandChannel *channel = NULL;
+  bool found = false;
   for (list<DeviceItem>::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
     if (ite->info.handle == handle) {
+      found = true;
       channel = ite->channel.get();
       break;
     }
   }
-
-  if (channel) {
-    channel->SendAsync(command);
-    return true;
+  if (!found) {
+    return kStatusInvalidHandle;
   }
-  return false;
+  if (!channel) {
+    return kStatusChannelNotExist;
+  }
+  channel->SendAsync(command);
+
+  return kStatusSuccess;
 }
 
 bool LidarCommandHandlerImpl::RemoveDevice(uint8_t handle) {

sdk_core/src/command_handler/lidar_command_handler.h

@@ -38,7 +38,7 @@ class LidarCommandHandlerImpl : public CommandHandlerImpl {
 
   bool AddDevice(const DeviceInfo &info);
   bool RemoveDevice(uint8_t handle);
-  bool SendCommand(uint8_t handle, const Command &command);
+  livox_status SendCommand(uint8_t handle, const Command &command);
 
  private:
   typedef struct {

sdk_core/src/data_handler/data_handler.cpp

@@ -29,8 +29,6 @@
 
 namespace livox {
 
-static const size_t kSphericalCoordinatePointSize = 9;
-static const size_t kCartesianCoordinatePointSize = 13;
 static const size_t kPrefixDataSize = 18;
 
 DataHandler &data_handler() {
@@ -91,12 +89,30 @@ void DataHandler::OnDataCallback(uint8_t handle, void *data, uint16_t size) {
     return;
   }
   DataCallback cb = callbacks_[handle];
-  if (lidar_data->data_type == 0) {
-    size = (size - kPrefixDataSize) / kCartesianCoordinatePointSize;
-  } else if (lidar_data->data_type == 1) {
-    size = (size - kPrefixDataSize) / kSphericalCoordinatePointSize;
-  } else {
-    return;
+  switch (lidar_data->data_type) {
+    case kCartesian:
+      size = (size - kPrefixDataSize) / sizeof(LivoxRawPoint);
+      break;
+    case kSpherical:
+      size = (size - kPrefixDataSize) / sizeof(LivoxSpherPoint);
+      break;
+    case kExtendCartesian:
+      size = (size - kPrefixDataSize) / sizeof(LivoxExtendRawPoint);
+      break;
+    case kExtendSpherical:
+      size = (size - kPrefixDataSize) / sizeof(LivoxExtendSpherPoint);
+      break;
+    case kDualExtendCartesian:
+      size = (size - kPrefixDataSize) / sizeof(LivoxDualExtendRawPoint);
+      break;
+    case kDualExtendSpherical:
+      size = (size - kPrefixDataSize) / sizeof(LivoxDualExtendSpherPoint);
+      break;
+    case kImu:
+      size = (size - kPrefixDataSize) / sizeof(LivoxImuPoint);
+      break;
+    default: 
+      return;
   }
   if (cb) {
     //LOG_INFO(" device_sn: {}",  device_sn);

sdk_core/src/device_discovery.cpp

@@ -66,7 +66,7 @@ bool DeviceDiscovery::Start(IOLoop *loop) {
     return false;
   }
   loop_ = loop;
-  sock_ = util::CreateBindSocket(kListenPort, mem_pool_);
+  sock_ = util::CreateBindSocket(kListenPort, mem_pool_, true);
   if (sock_ == NULL) {
     LOG_ERROR("DeviceDiscovery Create Socket Failed");
     return false;
@@ -84,7 +84,7 @@ void DeviceDiscovery::OnData(apr_socket_t *sock, void *) {
 
   comm_port_->UpdateCacheWrIdx(size);
   if (rv != APR_SUCCESS) {
-    LOG_ERROR(" Receive Failed {}", PrintAPRStatus(rv));
+    LOG_WARN(" Receive Failed {}", PrintAPRStatus(rv));
     return;
   }
   CommPacket packet;
@@ -137,6 +137,7 @@ void DeviceDiscovery::OnTimer(apr_time_t now) {
 
 void DeviceDiscovery::Uninit() {
   if (sock_) {
+    loop_->RemoveDelegate(sock_, this);
     apr_socket_close(sock_);
     sock_ = NULL;
   }
@@ -156,24 +157,10 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
     return;
   }
 
-  BroadcastDeviceInfo *device_info = (BroadcastDeviceInfo *)packet.data;
-  string broadcast_code = device_info->broadcast_code;
-  LOG_INFO(" Boradcast broadcast code: {}", broadcast_code);
-  device_manager().BroadcastDevices(device_info);
-  DeviceInfo lidar_info;
-  bool found = device_manager().FindDevice(broadcast_code, lidar_info);
-  if (!found || device_manager().IsDeviceConnected(lidar_info.handle)) {
-    return;
-  }
-
-  ++port_count;
-  strncpy(lidar_info.broadcast_code, broadcast_code.c_str(), sizeof(lidar_info.broadcast_code));
-  lidar_info.cmd_port = kListenPort + kCmdPortOffset + port_count;
-  lidar_info.data_port = kListenPort + kDataPortOffset + port_count;
-  lidar_info.type = device_info->dev_type;
-  lidar_info.state = kLidarStateUnknown;
-  lidar_info.feature = kLidarFeatureNone;
-  lidar_info.status.status_code = 0;
+  BroadcastDeviceInfo device_info;
+  memcpy((void*)(&device_info),(void*)(packet.data),(sizeof(BroadcastDeviceInfo)-sizeof(device_info.ip)));
+  string broadcast_code = device_info.broadcast_code;
+  LOG_INFO(" Broadcast broadcast code: {}", broadcast_code);
 
   char ip[16];
   memset(&ip, 0, sizeof(ip));
@@ -182,6 +169,31 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
     LOG_ERROR(PrintAPRStatus(rv));
     return;
   }
+  strncpy(device_info.ip, ip, sizeof(device_info.ip));
+  
+  device_manager().BroadcastDevices(&device_info);
+ 
+  DeviceInfo lidar_info;
+  bool found = device_manager().FindDevice(broadcast_code, lidar_info);
+
+  if (!found) {
+    LOG_INFO("Broadcast code : {} not add to connect", broadcast_code);
+  }
+
+  if (!found || device_manager().IsDeviceConnected(lidar_info.handle)) {
+    return;
+  }
+
+  ++port_count;
+  strncpy(lidar_info.broadcast_code, broadcast_code.c_str(), sizeof(lidar_info.broadcast_code));
+  lidar_info.cmd_port = kListenPort + kCmdPortOffset + port_count;
+  lidar_info.data_port = kListenPort + kDataPortOffset + port_count;
+  lidar_info.sensor_port = kListenPort + kSensorPortOffset + port_count;
+  lidar_info.type = device_info.dev_type;
+  lidar_info.state = kLidarStateUnknown;
+  lidar_info.feature = kLidarFeatureNone;
+  lidar_info.status.progress = 0;
+
   strncpy(lidar_info.ip, ip, sizeof(lidar_info.ip));
   apr_pool_t *pool = NULL;
   rv = apr_pool_create(&pool, mem_pool_);
@@ -206,11 +218,12 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
   do {
     HandshakeRequest handshake_req;
     uint32_t local_ip = 0;
-    if (util::FindLocalIP(addr->sa.sin, local_ip) == false) {
+    if (util::FindLocalIp(addr->sa.sin, local_ip) == false) {
       result = false;
+      LOG_INFO("LocalIp and DeviceIp are not in same subnet");
       break;
     }
-    LOG_INFO(" LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
+    LOG_INFO("LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
     LOG_INFO("Command Port: {}", lidar_info.cmd_port);
     LOG_INFO("Data Port: {}", lidar_info.data_port);
     CommPacket packet;
@@ -218,6 +231,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
     handshake_req.ip_addr = local_ip;
     handshake_req.cmd_port = lidar_info.cmd_port;
     handshake_req.data_port = lidar_info.data_port;
+    handshake_req.sensor_port = lidar_info.sensor_port;
     packet.packet_type = kCommandTypeAck;
     packet.seq_num = CommandChannel::GenerateSeq();
     packet.cmd_set = kCommandSetGeneral;
@@ -245,6 +259,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
   }
 }
 
+
 DeviceDiscovery &device_discovery() {
   static DeviceDiscovery discovery;
   return discovery;

sdk_core/src/device_discovery.h

@@ -71,6 +71,9 @@ class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate {
   static const apr_port_t kCmdPortOffset = 500;
   /** data port number start offset. */
   static const apr_port_t kDataPortOffset = 1000;
+    /** sensor port number start offset. */
+  static const apr_port_t kSensorPortOffset = 1000;
+
 
   static uint16_t port_count;
   apr_socket_t *sock_;

sdk_core/src/device_manager.cpp

@@ -61,6 +61,7 @@ void DeviceManager::Uninit() {
   connected_cb_ = NULL;
   device_mode_ = kDeviceModeNone;
 
+  lock_guard<mutex> lock(mutex_);
   for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
     ite->clear();
   }
@@ -93,8 +94,14 @@ bool DeviceManager::AddDevice(const DeviceInfo &device) {
 
 void DeviceManager::RemoveDevice(uint8_t handle) {
   lock_guard<mutex> lock(mutex_);
-  if (handle < devices_.size()) {
-    devices_[handle].connected = false;
+  if (device_mode_ == kDeviceModeHub) {
+    for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
+      ite->connected = false;
+    }
+  } else if (device_mode_ == kDeviceModeLidar) {
+    if (handle < devices_.size()) {
+      devices_[handle].connected = false;
+    }
   }
   LOG_INFO(" Device {} removed ", (uint16_t)handle);
 }
@@ -111,7 +118,7 @@ void DeviceManager::UpdateDevices(const DeviceInfo &device, DeviceEvent type) {
   }
 
   if (device_mode_ == kDeviceModeHub) {
-    if (type == kEventConnect) {
+    if (type == kEventHubConnectionChange) {
       LOG_DEBUG("Send Query lidars command");
       command_handler().SendCommand(kHubDefaultHandle,
                                     kCommandSetHub,
@@ -120,16 +127,23 @@ void DeviceManager::UpdateDevices(const DeviceInfo &device, DeviceEvent type) {
                                     0,
                                     MakeCommandCallback<DeviceManager, HubQueryLidarInformationResponse>(
                                         this, &DeviceManager::HubLidarInfomationCallback));
-    } else if (connected_cb_) {
-      connected_cb_(&device, type);
+    } else {
+      if (connected_cb_) {
+        connected_cb_(&device, type);
+      }
     }
   }
 }
 
-void DeviceManager::HubLidarInfomationCallback(uint8_t status, uint8_t, HubQueryLidarInformationResponse *response) {
+void DeviceManager::HubLidarInfomationCallback(livox_status status, uint8_t, HubQueryLidarInformationResponse *response) {
   if (status == kStatusSuccess) {
     {
       lock_guard<mutex> lock(mutex_);
+      for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
+        if (ite->info.handle != kHubDefaultHandle) {
+          ite->connected = false;
+        }
+      }
       for (int i = 0; i < response->count; i++) {
         std::size_t index = (response->device_info_list[i].slot - 1) * 3 + response->device_info_list[i].id - 1;
         if (index < devices_.size()) {
@@ -142,7 +156,7 @@ void DeviceManager::HubLidarInfomationCallback(uint8_t status, uint8_t, HubQuery
       }
     }
     if (connected_cb_) {
-      connected_cb_(&(devices_[kHubDefaultHandle].info), kEventConnect);
+      connected_cb_(&(devices_[kHubDefaultHandle].info), kEventHubConnectionChange);
     }
   } else {
     LOG_ERROR("Failed to query lidars information connected to hub.");
@@ -190,7 +204,6 @@ bool DeviceManager::FindDevice(uint8_t handle, DeviceInfo &info) {
   if (handle >= devices_.size()) {
     return false;
   }
-
   info = devices_[handle].info;
   return true;
 }
@@ -230,12 +243,20 @@ void DeviceManager::UpdateDeviceState(uint8_t handle, const HeartbeatResponse &r
     info.feature = static_cast<LidarFeature>(response.feature);
     update = true;
   }
-  if (info.status.progress != response.error_union.progress) {
-    LOG_INFO(
-        " Update progress {}, device connect {}", (uint16_t)response.error_union.progress, devices_[handle].connected);
-    info.status.progress = response.error_union.progress;
-    update = true;
+  if (response.state == kLidarStateInit) {
+    if (info.status.progress != response.error_union.progress) {
+      LOG_INFO(
+          " Update progress {}, device connect {}", (uint16_t)response.error_union.progress, devices_[handle].connected);
+      info.status.progress = response.error_union.progress;
+      update = true;
+    }
+  } else {
+    if (info.status.status_code.error_code != response.error_union.status_code.error_code) {
+      info.status.status_code.error_code = response.error_union.status_code.error_code;
+      update = true;
+    }
   }
+ 
   if (devices_[handle].connected && update == true) {
     if (connected_cb_) {
       connected_cb_(&info, kEventStateChange);
@@ -243,6 +264,15 @@ void DeviceManager::UpdateDeviceState(uint8_t handle, const HeartbeatResponse &r
   }
 }
 
+bool DeviceManager::IsLidarMid40(uint8_t handle) {
+  DeviceInfo lidar_info;
+  bool found = device_manager().FindDevice(handle, lidar_info);
+  if ( found && lidar_info.type == kDeviceTypeLidarMid40) {
+    return true;
+  }
+  return false;
+}
+
 DeviceManager &device_manager() {
   static DeviceManager lidar_manager;
   return lidar_manager;
@@ -252,17 +282,21 @@ void DeviceFound(const DeviceInfo &lidar_data) {
   device_manager().AddDevice(lidar_data);
   command_handler().AddDevice(lidar_data);
   data_handler().AddDevice(lidar_data);
-  device_manager().UpdateDevices(lidar_data, kEventConnect);
+  if (device_manager().device_mode() == kDeviceModeHub) {
+    device_manager().UpdateDevices(lidar_data, kEventHubConnectionChange);
+  } else {
+    device_manager().UpdateDevices(lidar_data, kEventConnect);
+  }
 }
 
-void DeviceRemove(uint8_t handle) {
+void DeviceRemove(uint8_t handle, DeviceEvent device_event) {
   DeviceInfo info;
   bool found = device_manager().FindDevice(handle, info);
   device_manager().RemoveDevice(handle);
   command_handler().RemoveDevice(handle);
   data_handler().RemoveDevice(handle);
   if (found) {
-    device_manager().UpdateDevices(info, kEventDisconnect);
+    device_manager().UpdateDevices(info, device_event);
   }
 }
 

sdk_core/src/device_manager.h

@@ -94,13 +94,14 @@ class DeviceManager : public noncopyable {
   void SetDeviceBroadcastCallback(const boost::function<void(const BroadcastDeviceInfo *info)> &cb) {
     broadcast_cb_ = cb;
   }
-  void HubLidarInfomationCallback(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response);
+  void HubLidarInfomationCallback(livox_status status, uint8_t handle, HubQueryLidarInformationResponse *response);
   DeviceMode device_mode() { return device_mode_; }
   void BroadcastDevices(const BroadcastDeviceInfo *info);
   void UpdateDevices(const DeviceInfo &device, DeviceEvent type);
   void UpdateDeviceState(uint8_t handle, const HeartbeatResponse &response);
   void GetConnectedDevices(std::vector<DeviceInfo> &devices);
   bool AddListeningDevice(const std::string &broadcast_code, DeviceMode mode, uint8_t &handle);
+  bool IsLidarMid40(uint8_t handle);
 
  private:
   typedef struct _DetailDeviceInfo {
@@ -149,7 +150,7 @@ class DeviceManager : public noncopyable {
 
 DeviceManager &device_manager();
 void DeviceFound(const DeviceInfo &data);
-void DeviceRemove(uint8_t handle);
+void DeviceRemove(uint8_t handle,DeviceEvent device_event);
 
 }  // namespace livox
 

sdk_core/src/livox_sdk.cpp

@@ -30,6 +30,7 @@
 #include "device_manager.h"
 using namespace livox;
 IOThread *g_thread = NULL;
+static bool is_initialized = false;
 
 void GetLivoxSdkVersion(LivoxSdkVersion *version) {
   if (version != NULL) {
@@ -40,6 +41,10 @@ void GetLivoxSdkVersion(LivoxSdkVersion *version) {
 }
 
 bool Init() {
+  if (is_initialized) {
+    return false;
+  }
+
   bool result = false;
   do {
     InitLogger();
@@ -78,10 +83,14 @@ bool Init() {
     apr_terminate();
   }
 
+  is_initialized = result;
   return result;
 }
 
 void Uninit() {
+  if (!is_initialized) {
+    return;
+  }
   if (g_thread) {
     g_thread->Quit();
     g_thread->Join();
@@ -97,9 +106,10 @@ void Uninit() {
     delete g_thread;
     g_thread = NULL;
   }
-
+  
   UninitLogger();
   apr_terminate();
+  is_initialized = false;
 }
 
 bool Start() {