Update livox_def.h

merge from master

README.md

@@ -233,5 +233,5 @@ Here is the example:
 # 5 Support
 
 You can get support from Livox with the following methods:
-* Send email to dev@livoxtech.com with a clear description of your problem and your setup
+* Send email to cs@livoxtech.com with a clear description of your problem and your setup
 * Github Issues

README_CN.md

@@ -234,5 +234,5 @@ char broadcast_code_list[kMaxLidarCount][kBroadcastCodeSize] = {
 # 5 支持
 
 你可以通过以下方式获取 Livox 的技术支持 :
-* 发送邮件到 dev@livoxtech.com 描述清楚问题和使用场景
+* 发送邮件到 cs@livoxtech.com 描述清楚问题和使用场景
 * Github Issues

sdk_core/include/config.h

@@ -41,7 +41,7 @@
     #define HAVE_KQUEUE 1
 #else
     #include <sys/poll.h>
-    #define HAVE POLL 1
+    #define HAVE_POLL 1
 #endif
 
 #endif  // CONFIG_H_

sdk_core/include/livox_def.h

@@ -141,7 +141,7 @@ typedef enum {
 
 #define LIVOX_SDK_MAJOR_VERSION       2
 #define LIVOX_SDK_MINOR_VERSION       3
-#define LIVOX_SDK_PATCH_VERSION       0
+#define LIVOX_SDK_PATCH_VERSION       1
 
 #define kBroadcastCodeSize 16
 
@@ -643,7 +643,7 @@ typedef struct {
   uint8_t month;
   uint8_t day;
   uint8_t hour;
-  uint32_t mircrosecond;
+  uint32_t microsecond;
 } LidarSetUtcSyncTimeRequest;
 
 /**

sdk_core/src/base/multiple_io/multiple_io_kqueue.cpp

@@ -83,22 +83,27 @@ bool MultipleIOKqueue::PollSetAdd(PollFd poll_fd) {
 
 bool MultipleIOKqueue::PollSetRemove(PollFd poll_fd) {
   int fd = poll_fd.fd;
+  bool result = true;
   if (descriptors_.find(fd) != descriptors_.end()) {
-    if (descriptors_[fd].event & READBLE_EVENT) {
+    do {
-      EV_SET(&kevent_, fd, EVFILT_READ, EV_DELETE, 0, 0, NULL);
+      if (descriptors_[fd].event & READBLE_EVENT) {
-      if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
+        EV_SET(&kevent_, fd, EVFILT_READ, EV_DELETE, 0, 0, NULL);
-        return false;
+        if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
+          result = false;
+          break;
+        }
       }
-    }
+      if (descriptors_[fd].event & WRITABLE_EVENT) {
-    if (descriptors_[fd].event & WRITABLE_EVENT) {
+        EV_SET(&kevent_, fd, EVFILT_WRITE, EV_DELETE, 0, 0, NULL);
-      EV_SET(&kevent_, fd, EVFILT_WRITE, EV_DELETE, 0, 0, NULL);
+        if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
-      if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
+          result = false;
-        return false;
+          break;
+        }
       }
-    }
+    } while(0);
     descriptors_.erase(fd);
   }
-  return true;
+  return result;
 }
 
 void MultipleIOKqueue::Poll(int time_out) {

sdk_core/src/base/multiple_io/multiple_io_poll.cpp

@@ -93,10 +93,10 @@ void MultipleIOPoll:: Poll(int time_out) {
     for (int i = 0; i < pollset_num_; i++) {
       FdEvent fd_event = NONE_EVENT;
       if (pollset_[i].revents & POLLIN) {
-        fd_event = | READBLE_EVENT;
+        fd_event |= READBLE_EVENT;
       }
       if (pollset_[i].revents & POLLOUT) {
-        fd_event = | WRITABLE_EVENT;
+        fd_event |= WRITABLE_EVENT;
       }
       int fd = pollset_[i].fd;
       if (descriptors_.find(fd) != descriptors_.end()) {

sdk_core/src/base/wake_up/unix/wake_up_pipe.cpp

@@ -67,30 +67,43 @@ bool WakeUpPipe::PipeDestroy() {
 }
 
 bool WakeUpPipe::PipeCreate() {
+  bool status = false;
   //in filedes[0]
   //out filedes[1]
-  int filedes[2];
+  int filedes[2]= {};
   if (pipe(filedes) == -1) {
     return false;
   }
+  do {
+    int flags = 0;
+    if ((flags = fcntl(filedes[0], F_GETFD)) == -1) {
+      break;
+    }
 
-  int flags = 0;
+    flags |= FD_CLOEXEC;
-  if ((flags = fcntl(filedes[0], F_GETFL|O_NONBLOCK)) == -1) {
+    if (fcntl(filedes[0], F_SETFD, flags) == -1) {
-    return false;
+      break;
-  }
+    }
 
-  flags |= FD_CLOEXEC;
+    flags = 0;
-  if (fcntl(filedes[0], F_SETFL, flags) == -1) {
+    if ((flags = fcntl(filedes[1], F_GETFD)) == -1) {
-    return false;
+      break;
-  }
+    }
 
-  flags = 0;
+    flags |= FD_CLOEXEC;
-  if ((flags = fcntl(filedes[1], F_GETFD)) == -1) {
+    if (fcntl(filedes[1], F_SETFD, flags) == -1) {
-    return false;
+      break;
-  }
+    }
+    status = true;
+  } while(0);
 
-  flags |= FD_CLOEXEC;
+  if (!status) {
-  if (fcntl(filedes[1], F_SETFD, flags) == -1) {
+    if (filedes[0] > 0) {
+      close(filedes[0]);
+    }
+    if (filedes[1] > 0) {
+      close(filedes[1]);
+    }
     return false;
   }
   pipe_out_ = filedes[0];

sdk_core/src/command_handler/command_channel.cpp

@@ -146,11 +146,7 @@ void CommandChannel::OnTimer(TimePoint now) {
     }
   }
 
-  if (now - last_heartbeat_ > std::chrono::seconds(3)) {
+  HeartBeat(now);
-    DeviceDisconnect(handle_);
-  } else {
-    HeartBeat(now);
-  }
 }
 
 void CommandChannel::Uninit() {

sdk_core/src/command_handler/command_impl.cpp

@@ -209,7 +209,7 @@ bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest*
   }
 
   utc_time_req->hour = hour;
-  utc_time_req->mircrosecond = (minute * 60 * 1000 + second * 1000) * 1000;
+  utc_time_req->microsecond = (minute * 60 * 1000 + second * 1000) * 1000;
   return true;
 }
 

sdk_core/src/device_discovery.cpp

@@ -30,6 +30,7 @@
 #include "base/logging.h"
 #include "base/network/network_util.h"
 #include "command_handler/command_impl.h"
+#include "command_handler/command_handler.h"
 #include "device_manager.h"
 #include "livox_def.h"
 
@@ -139,6 +140,52 @@ void DeviceDiscovery::Uninit() {
   }
 }
 
+
+void DeviceDiscovery::ReConnectDevice(const DeviceInfo& lidar_info, struct sockaddr *addr) {
+  HandshakeRequest handshake_req;
+  uint32_t local_ip = 0;
+  if (util::FindLocalIp(*(struct sockaddr_in*)addr, local_ip) == false) {
+    LOG_INFO("LocalIp and DeviceIp are not in same subnet");
+    LOG_INFO("LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
+    LOG_INFO("DeviceIP: {}", inet_ntoa(((struct sockaddr_in *)addr)->sin_addr));
+    return;
+  }
+  LOG_INFO("ReConnect Device");
+  LOG_INFO("LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
+  LOG_INFO("DeviceIP: {}", inet_ntoa(((struct sockaddr_in *)addr)->sin_addr));
+  LOG_INFO("Command Port: {}", lidar_info.cmd_port);
+  LOG_INFO("Data Port: {}", lidar_info.data_port);
+  handshake_req.ip_addr = local_ip;
+  handshake_req.cmd_port = lidar_info.cmd_port;
+  handshake_req.data_port = lidar_info.data_port;
+
+  command_handler().SendCommand(lidar_info.handle,
+                                kCommandSetGeneral,
+                                kCommandIDGeneralHandshake,
+                                (uint8_t*)&handshake_req,
+                                sizeof(HandshakeRequest),
+                                MakeCommandCallback<DeviceDiscovery, uint8_t>(
+                                    this, &DeviceDiscovery::OnHandshakeCallback));
+}
+
+void DeviceDiscovery::OnHandshakeCallback(livox_status status, uint8_t handle, uint8_t) {
+  if (status != kStatusSuccess) {
+    LOG_WARN("On HandshakeCallback status: {}", status);
+    return;
+  }
+  DeviceInfo lidar_info;
+  bool found = device_manager().FindDevice(handle, lidar_info);
+  if (!found) {
+    return;
+  }
+
+  if (device_manager().device_mode() == kDeviceModeHub) {
+    device_manager().UpdateDevices(lidar_info, kEventHubConnectionChange);
+  } else {
+    device_manager().UpdateDevices(lidar_info, kEventConnect);
+  }
+}
+
 void DeviceDiscovery::OnBroadcast(const CommPacket &packet,  struct sockaddr *addr) {
   if (packet.data == NULL) {
     return;
@@ -161,9 +208,11 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet,  struct sockaddr *ad
 
   if (!found) {
     LOG_INFO("Broadcast code : {} not add to connect", broadcast_code);
+    return;
   }
 
-  if (!found || device_manager().IsDeviceConnected(lidar_info.handle)) {
+  if (device_manager().IsDeviceConnected(lidar_info.handle)) {
+    ReConnectDevice(lidar_info, addr);
     return;
   }
 

sdk_core/src/device_discovery.h

@@ -71,7 +71,9 @@ class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate {
   void OnTimer(TimePoint now);
 
  private:
+  void ReConnectDevice(const DeviceInfo& lidar_info, struct sockaddr *addr);
   void OnBroadcast(const CommPacket &packet, struct sockaddr *addr);
+  void OnHandshakeCallback(livox_status status, uint8_t handle, uint8_t);
 
  private:
   /** broadcast listening port number. */

sdk_core/src/device_manager.cpp

@@ -160,6 +160,18 @@ void DeviceManager::HubLidarInfomationCallback(livox_status status, uint8_t, Hub
     if (connected_cb_) {
       connected_cb_(&(devices_[kHubDefaultHandle].info), kEventHubConnectionChange);
     }
+  } else if (status == kStatusTimeout) {
+    LOG_WARN("Query lidars information Timeout.");
+    if (IsDeviceConnected(kHubDefaultHandle)) {
+      LOG_INFO("Retry to query lidars information.");
+      command_handler().SendCommand(kHubDefaultHandle,
+                                    kCommandSetHub,
+                                    kCommandIDHubQueryLidarInformation,
+                                    NULL,
+                                    0,
+                                    MakeCommandCallback<DeviceManager, HubQueryLidarInformationResponse>(
+                                        this, &DeviceManager::HubLidarInfomationCallback));
+    }
   } else {
     LOG_ERROR("Failed to query lidars information connected to hub.");
   }
@@ -178,6 +190,18 @@ void DeviceManager::QueryDeviceInformationCallback(livox_status status, uint8_t
     if (connected_cb_) {
       connected_cb_(&devices_[handle].info, kEventConnect);
     }
+  } else if (status == kStatusTimeout) {
+    LOG_WARN("Query lidar information Timeout.");
+    if (IsDeviceConnected(handle)) {
+      LOG_INFO("Retry to query lidar information.");
+      command_handler().SendCommand(handle,
+                                    kCommandSetGeneral,
+                                    kCommandIDGeneralDeviceInfo,
+                                    NULL,
+                                    0,
+                                    MakeCommandCallback<DeviceManager, DeviceInformationResponse>(
+                                        this, &DeviceManager::QueryDeviceInformationCallback));
+    }
   } else {
     LOG_ERROR("Failed to query lidar information.");
   }