From 53872851d208adfa4bf31e5f0630479aacbdd989 Mon Sep 17 00:00:00 2001
From: Livox-SDK <dev@livoxtech.com>
Date: Tue, 20 Apr 2021 12:25:48 +0800
Subject: [PATCH] cancle heartbeat timeout

---
 .../src/command_handler/command_channel.cpp   |  6 +--
 sdk_core/src/device_discovery.cpp             | 51 ++++++++++++++++++-
 sdk_core/src/device_discovery.h               |  2 +
 3 files changed, 53 insertions(+), 6 deletions(-)

diff --git a/sdk_core/src/command_handler/command_channel.cpp b/sdk_core/src/command_handler/command_channel.cpp
index 913aaa9..5cd9103 100644
--- a/sdk_core/src/command_handler/command_channel.cpp
+++ b/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)) {
-    DeviceDisconnect(handle_);
-  } else {
-    HeartBeat(now);
-  }
+  HeartBeat(now);
 }
 
 void CommandChannel::Uninit() {
diff --git a/sdk_core/src/device_discovery.cpp b/sdk_core/src/device_discovery.cpp
index b3b70f1..1cae0f7 100644
--- a/sdk_core/src/device_discovery.cpp
+++ b/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;
   }
 
diff --git a/sdk_core/src/device_discovery.h b/sdk_core/src/device_discovery.h
index f0942b1..1045125 100644
--- a/sdk_core/src/device_discovery.h
+++ b/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. */