361 lines
11 KiB
C++
361 lines
11 KiB
C++
#ifndef LIVOXPROTO1_PROTOCOL_H
|
|
#define LIVOXPROTO1_PROTOCOL_H
|
|
|
|
#include <vector>
|
|
#include <string>
|
|
#include <memory>
|
|
#include <sstream>
|
|
#include <atomic>
|
|
#include <cstdint>
|
|
#include <boost/asio/ip/address_v4.hpp>
|
|
#include <user/senseApiDesc.h>
|
|
|
|
namespace livoxProto1 {
|
|
namespace comms {
|
|
|
|
/** EXPLANATION:
|
|
* Undocumented Livox SDK CRC seed constants. These were found in the Livox SDK
|
|
* source code.
|
|
*/
|
|
constexpr uint16_t LIVOX_CRC16_SEED = 0x4c49;
|
|
constexpr uint32_t LIVOX_CRC32_SEED = 0x564f580a;
|
|
|
|
// Endianness detection
|
|
namespace endian {
|
|
inline bool isLittleEndian() {
|
|
union {
|
|
uint32_t i;
|
|
char c[4];
|
|
} test = {0x01020304};
|
|
return test.c[0] == 4;
|
|
}
|
|
}
|
|
|
|
// IPv4 address validation
|
|
inline bool isValidIPv4(const std::string& ipAddress) {
|
|
boost::system::error_code ec;
|
|
(void)boost::asio::ip::make_address_v4(ipAddress, ec);
|
|
return !ec;
|
|
}
|
|
|
|
// CRC calculation utilities
|
|
uint16_t calculateCrc16(
|
|
const uint8_t* data, size_t length);
|
|
uint32_t calculateCrc32(
|
|
const uint8_t* data, size_t length);
|
|
|
|
// IP address parsing utility
|
|
struct IPOctets {
|
|
std::string octet1, octet2, octet3, octet4;
|
|
};
|
|
|
|
std::optional<IPOctets> parseIPv4Address(const std::string& ipAddress);
|
|
|
|
// Device identifier comparison
|
|
inline bool deviceIdentifiersEqual(
|
|
const std::string& id1, const std::string& id2
|
|
)
|
|
{
|
|
// Use pointers to avoid unnecessary string copies
|
|
const std::string* serial1_ptr;
|
|
const std::string* serial2_ptr;
|
|
|
|
// Local copies only needed for 15-character broadcast codes
|
|
std::string serial1_copy, serial2_copy;
|
|
|
|
// Determine if id1 is serial (14 chars) or broadcast code (15 chars)
|
|
if (id1.length() == 14) {
|
|
serial1_ptr = &id1; // No copy needed, use original string
|
|
} else if (id1.length() == 15) {
|
|
serial1_copy = id1.substr(0, 14); // Copy only when necessary
|
|
serial1_ptr = &serial1_copy;
|
|
} else {
|
|
return false; // Invalid length
|
|
}
|
|
|
|
// Determine if id2 is serial (14 chars) or broadcast code (15 chars)
|
|
if (id2.length() == 14) {
|
|
serial2_ptr = &id2; // No copy needed, use original string
|
|
} else if (id2.length() == 15) {
|
|
serial2_copy = id2.substr(0, 14); // Copy only when necessary
|
|
serial2_ptr = &serial2_copy;
|
|
} else {
|
|
return false; // Invalid length
|
|
}
|
|
|
|
// Compare the serial numbers using pointers
|
|
return *serial1_ptr == *serial2_ptr;
|
|
}
|
|
|
|
/** EXPLANATION:
|
|
* Device types as defined in the Livox protocol specification
|
|
*/
|
|
enum class DeviceType : uint8_t {
|
|
Hub = 0,
|
|
Mid40 = 1,
|
|
Tele15 = 2,
|
|
Horizon = 3,
|
|
Mid70 = 6,
|
|
Avia = 7
|
|
};
|
|
|
|
/** EXPLANATION:
|
|
* Protocol frame header structure.
|
|
* All multi-byte fields are in little-endian format as per protocol spec.
|
|
*/
|
|
struct Header
|
|
{
|
|
uint8_t sof; // 0: Start of Frame (0xAA)
|
|
uint8_t version; // 1: Protocol Version (1)
|
|
uint16_t length; // 2-3: Frame Length (little-endian)
|
|
uint8_t cmd_type; // 4: Command Type (0x02 = MSG for broadcast)
|
|
uint16_t seq_num; // 5-6: Sequence Number (little-endian)
|
|
uint16_t crc_16; // 7-8: Header Checksum (little-endian)
|
|
|
|
void swapToHostEndianness();
|
|
void swapToProtocolEndianness();
|
|
void swapCrc16ToHostEndianness();
|
|
void swapCrc16ToProtocolEndianness();
|
|
bool sanityCheck() const;
|
|
uint16_t calculateCrc16() const;
|
|
bool validateCrc16() const;
|
|
void setCrc16FromRawBytes();
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Protocol frame footer structure.
|
|
* All multi-byte fields are in little-endian format as per protocol spec.
|
|
*/
|
|
struct Footer
|
|
{
|
|
uint32_t crc_32; // 0-3: Whole Frame Checksum (little-endian)
|
|
|
|
void swapToHostEndianness();
|
|
void swapToProtocolEndianness();
|
|
void swapCrc32ToHostEndianness();
|
|
void swapCrc32ToProtocolEndianness();
|
|
bool validateCrc32() const;
|
|
bool sanityCheck() const;
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Command identification structure used in all Livox protocol messages.
|
|
* Contains the command set and command ID fields.
|
|
*/
|
|
struct Command
|
|
{
|
|
uint8_t cmd_set; // 0: Command Set (0x00 = General, etc.)
|
|
uint8_t cmd_id; // 1: Command ID (0x00 = Broadcast, 0x01 = Handshake, etc.)
|
|
|
|
void swapToHostEndianness();
|
|
void swapToProtocolEndianness();
|
|
bool sanityCheck() const;
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete wire format for Livox broadcast messages.
|
|
* All multi-byte fields are in little-endian format as per protocol spec.
|
|
*/
|
|
struct BroadcastMessage
|
|
{
|
|
Header header; // 0-8: Protocol frame header
|
|
Command command; // 9-10: Command identification
|
|
uint8_t broadcast_code[16]; // 11-26: Device Broadcast Code (null-terminated string)
|
|
uint8_t dev_type; // 27: Device Type
|
|
uint16_t reserved; // 28-29: Reserved (little-endian)
|
|
Footer footer; // 30-33: Protocol frame footer
|
|
|
|
void swapContentsToHostEndianness();
|
|
bool sanityCheck() const;
|
|
bool validateCrc32() const;
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete handshake request frame for connecting to Livox devices.
|
|
* This is the complete wire format including header, command fields, data, and footer.
|
|
*/
|
|
struct HandshakeRequest
|
|
{
|
|
Header header; // 0-8: Protocol frame header
|
|
Command command; // 9-10: Command identification
|
|
uint8_t user_ip[4]; // 11-14: Host IP Address (little-endian)
|
|
uint16_t data_port; // 15-16: Host Point Cloud Data UDP Destination Port (little-endian)
|
|
uint16_t cmd_port; // 17-18: Host Control Command UDP Destination Port (little-endian)
|
|
uint16_t imu_port; // 19-20: Host IMU UDP Destination Port (little-endian)
|
|
Footer footer; // 21-24: Protocol frame footer
|
|
|
|
HandshakeRequest(
|
|
const std::string& hostIP,
|
|
uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort);
|
|
|
|
// Calculate CRC32 for the entire message
|
|
uint32_t calculateCrc32() const;
|
|
void swapContentsToProtocolEndianness();
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete handshake response frame from Livox devices.
|
|
* This is the complete wire format including header, command fields, data, and footer.
|
|
*/
|
|
struct HandshakeResponse
|
|
{
|
|
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 heartbeat command frame for maintaining connection with Livox devices.
|
|
* This is the complete wire format including header, command fields, and footer.
|
|
*/
|
|
struct HeartbeatMessage
|
|
{
|
|
Header header; // 0-8: Protocol frame header
|
|
Command command; // 9-10: Command identification
|
|
Footer footer; // 11-14: Protocol frame footer
|
|
|
|
HeartbeatMessage();
|
|
uint32_t calculateCrc32() const;
|
|
void swapContentsToProtocolEndianness();
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete disconnect command frame for disconnecting from Livox devices.
|
|
* This is the complete wire format including header, command fields, and footer.
|
|
*/
|
|
struct DisconnectMessage
|
|
{
|
|
Header header; // 0-8: Protocol frame header
|
|
Command command; // 9-10: Command identification
|
|
Footer footer; // 11-14: Protocol frame footer
|
|
|
|
DisconnectMessage();
|
|
uint32_t calculateCrc32() const;
|
|
void swapContentsToProtocolEndianness();
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete start/stop sampling command frame for enabling/disabling point cloud data from Livox devices.
|
|
* This is the complete wire format including header, command fields, data, and footer.
|
|
*/
|
|
struct StartStopSamplingMessage
|
|
{
|
|
Header header; // 0-8: Protocol frame header
|
|
Command command; // 9-10: Command identification
|
|
uint8_t enable; // 11: Enable flag (0x01 = Start, 0x00 = Stop)
|
|
Footer footer; // 12-15: Protocol frame footer
|
|
|
|
StartStopSamplingMessage();
|
|
uint32_t calculateCrc32() const;
|
|
void swapContentsToProtocolEndianness();
|
|
} __attribute__((packed));
|
|
|
|
/** EXPLANATION:
|
|
* Complete sampling response frame from Livox devices.
|
|
* This is the complete wire format including header, command fields, data, and footer.
|
|
*/
|
|
struct SamplingResponse
|
|
{
|
|
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 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));
|
|
|
|
/** 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();
|
|
} __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();
|
|
} __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
|
|
|
|
#endif // LIVOXPROTO1_PROTOCOL_H
|