LivoxGen1: Add Get/SetReturn rate proto headers

2025-10-24 22:11:31 -04:00
parent dc864bad00
commit 3373393755
2 changed files with 224 additions and 0 deletions
@@ -838,5 +838,161 @@ bool HeartbeatACK::validateCrc32() const
 	return isValid;
 }

+// SetLiDARReturnMode methods
+SetLiDARReturnMode::SetLiDARReturnMode()
+{
+	// Initialize header
+	header.sof = 0xAA;
+	header.version = 0x01;
+	header.length = sizeof(SetLiDARReturnMode);
+	header.crc_16 = 0; // Will be calculated later
+
+	// Initialize command
+	command.cmd_set = 0x01; // LiDAR Command
+	command.cmd_id = 0x06;  // Set LiDAR Return Mode
+
+	// Initialize mode (default to Single Return First)
+	mode = 0x00;
+
+	// Initialize footer
+	footer.crc_32 = 0; // Will be calculated later
+}
+
+uint32_t SetLiDARReturnMode::calculateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(SetLiDARReturnMode) - sizeof(footer.crc_32);
+	return comms::calculateCrc32(messageData, messageSize);
+}
+
+void SetLiDARReturnMode::swapContentsToProtocolEndianness()
+{
+	if (endian::isLittleEndian()) { return; }
+	header.swapToProtocolEndianness();
+	command.swapToProtocolEndianness();
+	// mode is uint8_t, no endianness conversion needed
+	footer.swapToProtocolEndianness();
+}
+
+bool SetLiDARReturnMode::sanityCheck() const
+{
+	return header.sanityCheck() &&
+		command.sanityCheck() &&
+		(command.cmd_set == 0x01) && (command.cmd_id == 0x06) &&
+		(mode <= 0x03) && // Valid modes: 0x00-0x03
+		footer.sanityCheck();
+}
+
+bool SetLiDARReturnMode::validateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(SetLiDARReturnMode) - sizeof(footer.crc_32);
+	uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
+	return (calculatedCrc == footer.crc_32);
+}
+
+// SetLiDARReturnModeResponse methods
+void SetLiDARReturnModeResponse::swapContentsToHostEndianness()
+{
+	if (endian::isLittleEndian()) { return; }
+	header.swapToHostEndianness();
+	command.swapToHostEndianness();
+	// ret_code is uint8_t, no endianness conversion needed
+	// Note: footer.swapToHostEndianness() swaps CRC, so we skip it here
+}
+
+bool SetLiDARReturnModeResponse::sanityCheck() const
+{
+	return header.sanityCheck() &&
+		command.sanityCheck() &&
+		(command.cmd_set == 0x01) && (command.cmd_id == 0x06) &&
+		(ret_code <= 0x01) && // Valid return codes: 0x00-0x01
+		footer.sanityCheck();
+}
+
+bool SetLiDARReturnModeResponse::validateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(SetLiDARReturnModeResponse) - sizeof(footer.crc_32);
+	uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
+	return (calculatedCrc == footer.crc_32);
+}
+
+// GetLiDARReturnMode methods
+GetLiDARReturnMode::GetLiDARReturnMode()
+{
+	// Initialize header
+	header.sof = 0xAA;
+	header.version = 0x01;
+	header.length = sizeof(GetLiDARReturnMode);
+	header.crc_16 = 0; // Will be calculated later
+
+	// Initialize command
+	command.cmd_set = 0x01; // LiDAR Command
+	command.cmd_id = 0x07;  // Get LiDAR Return Mode
+
+	// Initialize footer
+	footer.crc_32 = 0; // Will be calculated later
+}
+
+uint32_t GetLiDARReturnMode::calculateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(GetLiDARReturnMode) - sizeof(footer.crc_32);
+	return comms::calculateCrc32(messageData, messageSize);
+}
+
+void GetLiDARReturnMode::swapContentsToProtocolEndianness()
+{
+	if (endian::isLittleEndian()) { return; }
+	header.swapToProtocolEndianness();
+	command.swapToProtocolEndianness();
+	footer.swapToProtocolEndianness();
+}
+
+bool GetLiDARReturnMode::sanityCheck() const
+{
+	return header.sanityCheck() &&
+		command.sanityCheck() &&
+		(command.cmd_set == 0x01) && (command.cmd_id == 0x07) &&
+		footer.sanityCheck();
+}
+
+bool GetLiDARReturnMode::validateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(GetLiDARReturnMode) - sizeof(footer.crc_32);
+	uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
+	return (calculatedCrc == footer.crc_32);
+}
+
+// GetLiDARReturnModeResponse methods
+void GetLiDARReturnModeResponse::swapContentsToHostEndianness()
+{
+	if (endian::isLittleEndian()) { return; }
+	header.swapToHostEndianness();
+	command.swapToHostEndianness();
+	// ret_code and mode are uint8_t, no endianness conversion needed
+	// Note: footer.swapToHostEndianness() swaps CRC, so we skip it here
+}
+
+bool GetLiDARReturnModeResponse::sanityCheck() const
+{
+	return header.sanityCheck() &&
+		command.sanityCheck() &&
+		(command.cmd_set == 0x01) && (command.cmd_id == 0x07) &&
+		(ret_code <= 0x01) && // Valid return codes: 0x00-0x01
+		(mode <= 0x03) && // Valid modes: 0x00-0x03
+		footer.sanityCheck();
+}
+
+bool GetLiDARReturnModeResponse::validateCrc32() const
+{
+	const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
+	size_t messageSize = sizeof(GetLiDARReturnModeResponse) - sizeof(footer.crc_32);
+	uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
+	return (calculatedCrc == footer.crc_32);
+}
+
 } // namespace comms
 } // namespace livoxProto1