LivoxProto1: Print when Lidar isn't ready for work

2025-10-24 00:51:28 -04:00
parent 83c937ae8f
commit bede123691
3 changed files with 126 additions and 2 deletions
@@ -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()