diff --git a/commonLibs/livoxProto1/protocol.cpp b/commonLibs/livoxProto1/protocol.cpp index 053f4ee..a9b2480 100644 --- a/commonLibs/livoxProto1/protocol.cpp +++ b/commonLibs/livoxProto1/protocol.cpp @@ -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(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(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(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(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(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(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 diff --git a/commonLibs/livoxProto1/protocol.h b/commonLibs/livoxProto1/protocol.h index 32827f6..ca1289b 100644 --- a/commonLibs/livoxProto1/protocol.h +++ b/commonLibs/livoxProto1/protocol.h @@ -299,6 +299,74 @@ struct HeartbeatACK bool validateCrc32() const; } __attribute__((packed)); +/** EXPLANATION: + * Complete set LiDAR return mode command frame for Livox devices. + * This is the complete wire format including header, command fields, data, and footer. + */ +struct SetLiDARReturnMode +{ + Header header; // 0-8: Protocol frame header + Command command; // 9-10: Command identification + uint8_t mode; // 11: Return Mode (0x00: Single Return First, 0x01: Single Return Strongest, 0x02: Dual Return, 0x03: Triple Return) + Footer footer; // 12-15: Protocol frame footer + + SetLiDARReturnMode(); + uint32_t calculateCrc32() const; + void swapContentsToProtocolEndianness(); + bool sanityCheck() const; + bool validateCrc32() const; +} __attribute__((packed)); + +/** EXPLANATION: + * Complete set LiDAR return mode response frame from Livox devices. + * This is the complete wire format including header, command fields, data, and footer. + */ +struct SetLiDARReturnModeResponse +{ + Header header; // 0-8: Protocol frame header + Command command; // 9-10: Command identification + uint8_t ret_code; // 11: Return Code (0x00 = Success, 0x01 = Fail) + Footer footer; // 12-15: Protocol frame footer + + void swapContentsToHostEndianness(); + bool sanityCheck() const; + bool validateCrc32() const; +} __attribute__((packed)); + +/** EXPLANATION: + * Complete get LiDAR return mode command frame for Livox devices. + * This is the complete wire format including header, command fields, data, and footer. + */ +struct GetLiDARReturnMode +{ + Header header; // 0-8: Protocol frame header + Command command; // 9-10: Command identification + Footer footer; // 11-14: Protocol frame footer + + GetLiDARReturnMode(); + uint32_t calculateCrc32() const; + void swapContentsToProtocolEndianness(); + bool sanityCheck() const; + bool validateCrc32() const; +} __attribute__((packed)); + +/** EXPLANATION: + * Complete get LiDAR return mode response frame from Livox devices. + * This is the complete wire format including header, command fields, data, and footer. + */ +struct GetLiDARReturnModeResponse +{ + 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 mode; // 12: Return Mode (0x00: Single Return First, 0x01: Single Return Strongest, 0x02: Dual Return, 0x03: Triple Return) + Footer footer; // 13-16: Protocol frame footer + + void swapContentsToHostEndianness(); + bool sanityCheck() const; + bool validateCrc32() const; +} __attribute__((packed)); + } // namespace comms } // namespace livoxProto1