diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp index 028f150..4a613c6 100644 --- a/commonLibs/livoxProto1/device.cpp +++ b/commonLibs/livoxProto1/device.cpp @@ -1042,6 +1042,8 @@ void Device::startHeartbeat() } // Register heartbeat ACK handler (cmd_set=0x00, cmd_id=0x03) + smo::SpinLock::Guard lock(heartbeatActiveLock); + registerUdpCommandHandler( 0x00, 0x03, discardHeartbeatAck, discoveredDevice.ipAddr); @@ -1057,15 +1059,17 @@ void Device::startHeartbeat() void Device::stopHeartbeat() { - if (heartbeatActive.load()) { - heartbeatActive.store(false); - if (heartbeatTimer) { - heartbeatTimer->cancel(); - } + smo::SpinLock::Guard lock(heartbeatActiveLock); + heartbeatActive.store(false); unregisterUdpCommandHandler(0x00, 0x03, discoveredDevice.ipAddr); } + + if (heartbeatTimer) { + heartbeatTimer->cancel(); + heartbeatTimer.reset(); + } } void Device::sendHeartbeat() @@ -1140,7 +1144,6 @@ void Device::sendHeartbeat() } catch (const std::exception& e) { - heartbeatActive.store(false); std::cerr << __func__ << ": Heartbeat send failed for device " << discoveredDevice.deviceIdentifier << ": " << e.what() << std::endl; @@ -1156,15 +1159,21 @@ void Device::onHeartbeatTimer(const boost::system::error_code& error) if (error) { - heartbeatActive.store(false); std::cerr << "[" << __func__ << "] Heartbeat timer error for device " << discoveredDevice.deviceIdentifier << ": " << error.message() << std::endl; + return; } // Send next heartbeat - sendHeartbeat(); + { + smo::SpinLock::Guard lock(heartbeatActiveLock); + if (!heartbeatActive.load()) + { return; } + + sendHeartbeat(); + } } uint32_t Device::getSubnetMaskFor(uint8_t nbits) diff --git a/commonLibs/livoxProto1/device.h b/commonLibs/livoxProto1/device.h index 563e79c..a066eb9 100644 --- a/commonLibs/livoxProto1/device.h +++ b/commonLibs/livoxProto1/device.h @@ -17,6 +17,7 @@ #include #include "protocol.h" #include +#include // Custom hash function for std::pair namespace std { @@ -162,6 +163,7 @@ public: // Heartbeat state std::unique_ptr heartbeatTimer; std::atomic heartbeatActive; + smo::SpinLock heartbeatActiveLock; // Point cloud data state std::atomic pcloudDataActive;