hayodea
5845f1a41d
This symbol is defined as a static member object inside of a boost detail header. When boost headers are used in a project that uses Boost in both the main binary as well as dlopen()'d shlibs, the top_ symbol gets duplicated and the metadata gets partitioned. We use the Boost shlib to unify both the main binary and the shlibs to use the same memory address for top_. This involves marking the templated object call_stack::top_ as "extern" and then declaring to Boost that we intend to use the shlibs.
362 lines
11 KiB
C++
362 lines
11 KiB
C++
#ifndef LIVOXPROTO1_PROTOCOL_H
|
|
#define LIVOXPROTO1_PROTOCOL_H
|
|
|
|
#include <boostAsioLinkageFix.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;
|
|
boost::asio::ip::address_v4::from_string(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
|