From bede123691f8861b74fe15a2dfd07b904f16dc93 Mon Sep 17 00:00:00 2001
From: Hayodea Hekol <hayodea@protonmail.com>
Date: Fri, 24 Oct 2025 00:51:28 -0400
Subject: [PATCH] LivoxProto1: Print when Lidar isn't ready for work

---
 commonLibs/livoxProto1/device.cpp   | 71 ++++++++++++++++++++++++++++-
 commonLibs/livoxProto1/protocol.cpp | 38 +++++++++++++++
 commonLibs/livoxProto1/protocol.h   | 19 ++++++++
 3 files changed, 126 insertions(+), 2 deletions(-)

diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp
index 2450c46..31449a0 100644
--- a/commonLibs/livoxProto1/device.cpp
+++ b/commonLibs/livoxProto1/device.cpp
@@ -981,9 +981,76 @@ static void discardHeartbeatAck(
 	const struct sockaddr_in& senderAddr
 	)
 {
-	(void)data;
-	(void)bytesReceived;
 	(void)senderAddr;
+
+	// Check if we have enough data for a HeartbeatACK message
+	if (bytesReceived
+		< static_cast<ssize_t>(sizeof(livoxProto1::comms::HeartbeatACK)))
+	{
+		std::cout << __func__ << ": Received heartbeat ACK with insufficient "
+			"data (" << bytesReceived << " bytes, expected "
+			<< sizeof(livoxProto1::comms::HeartbeatACK) << ")" << std::endl;
+		return;
+	}
+
+	// Directly use a non-const reference to HeartbeatACK structure
+	livoxProto1::comms::HeartbeatACK& ack =
+		*reinterpret_cast<livoxProto1::comms::HeartbeatACK*>(
+			const_cast<uint8_t*>(data));
+
+	ack.swapContentsToHostEndianness();
+
+	if (!ack.validateCrc32())
+	{
+		std::cerr << __func__ << ": Discarded heartbeat ACK - CRC32 validation "
+			"failed" << std::endl;
+		return;
+	}
+
+	if (!ack.header.validateCrc16())
+	{
+		std::cerr << __func__ << ": Discarded heartbeat ACK - CRC16 validation "
+			"failed" << std::endl;
+		return;
+	}
+
+	if (!ack.sanityCheck())
+	{
+		std::cerr << __func__ << ": Discarded heartbeat ACK - sanity check "
+			"failed" << std::endl;
+		return;
+	}
+
+	if (ack.work_state == 0x01) { return; }
+
+	// Print work_state with human-readable description
+	std::string workStateStr;
+	switch (ack.work_state)
+	{
+		case 0x00:
+			workStateStr = "Initializing";
+			break;
+		case 0x01:
+			workStateStr = "Normal";
+			break;
+		case 0x02:
+			workStateStr = "Power-Saving";
+			break;
+		case 0x03:
+			workStateStr = "Standby";
+			break;
+		case 0x04:
+			workStateStr = "Error";
+			break;
+		default:
+			workStateStr = "Unknown";
+			break;
+	}
+
+	std::cerr << __func__ << ": Lidar not ready for operation: work_state: 0x"
+		<< std::hex << static_cast<int>(ack.work_state) << std::dec
+		<< " (" << workStateStr << "), ack_msg: 0x"
+		<< std::hex << ack.ack_msg << std::dec << std::endl;
 }
 
 void Device::startHeartbeat()
diff --git a/commonLibs/livoxProto1/protocol.cpp b/commonLibs/livoxProto1/protocol.cpp
index 317c36d..053f4ee 100644
--- a/commonLibs/livoxProto1/protocol.cpp
+++ b/commonLibs/livoxProto1/protocol.cpp
@@ -800,5 +800,43 @@ bool SamplingResponse::validateCrc32() const
 	return isValid;
 }
 
+// HeartbeatACK methods
+void HeartbeatACK::swapContentsToHostEndianness()
+{
+	if (endian::isLittleEndian()) { return; }
+	// Only swap content fields, not CRC fields
+	header.swapToHostEndianness();
+	command.swapToHostEndianness();
+	ack_msg = __builtin_bswap32(ack_msg);
+	// Note: footer.swapToHostEndianness() swaps CRC, so we skip it here
+}
+
+bool HeartbeatACK::sanityCheck() const
+{
+	return header.sanityCheck() &&
+		command.sanityCheck() &&
+		(command.cmd_set == 0x00) && (command.cmd_id == 0x03) &&
+		footer.sanityCheck();
+}
+
+bool HeartbeatACK::validateCrc32() const
+{
+	// Calculate CRC32 for the entire message excluding the footer.crc_32 field
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(HeartbeatACK) - sizeof(footer.crc_32);
+	uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
+
+	// Compare with the CRC in the footer
+	bool isValid = (calculatedCrc == footer.crc_32);
+
+	// Debug output only if validation fails
+	if (!isValid) {
+		std::cout << "HeartbeatACK CRC32 Debug: calculated=0x" << std::hex << calculatedCrc
+			<< ", received=0x" << footer.crc_32 << std::dec << std::endl;
+	}
+
+	return isValid;
+}
+
 } // namespace comms
 } // namespace livoxProto1
diff --git a/commonLibs/livoxProto1/protocol.h b/commonLibs/livoxProto1/protocol.h
index d564d16..32827f6 100644
--- a/commonLibs/livoxProto1/protocol.h
+++ b/commonLibs/livoxProto1/protocol.h
@@ -280,6 +280,25 @@ struct SamplingResponse
 	bool validateCrc32() const;
 } __attribute__((packed));
 
+/**		EXPLANATION:
+ * Complete heartbeat ACK response frame from Livox devices.
+ * This is the complete wire format including header, command fields, data, and footer.
+ */
+struct HeartbeatACK
+{
+	Header header;			// 0-8: Protocol frame header
+	Command command;		// 9-10: Command identification
+	uint8_t ret_code;		// 11: Return Code (0x00 = Success, 0x01 = Fail)
+	uint8_t work_state;		// 12: LiDAR/Hub State (0x00: Initializing, 0x01: Normal, 0x02: Power-Saving, 0x03: Standby, 0x04: Error)
+	uint8_t feature_msg;	// 13: LiDAR Feature Message (Bit0: Rain/Fog Suppression Switch)
+	uint32_t ack_msg;		// 14-17: ACK Message (Initialization Progress or Status Code)
+	Footer footer;			// 18-21: Protocol frame footer
+
+	void swapContentsToHostEndianness();
+	bool sanityCheck() const;
+	bool validateCrc32() const;
+} __attribute__((packed));
+
 } // namespace comms
 } // namespace livoxProto1