diff --git a/commonLibs/livoxProto1/CMakeLists.txt b/commonLibs/livoxProto1/CMakeLists.txt index dd72ab4..619248e 100644 --- a/commonLibs/livoxProto1/CMakeLists.txt +++ b/commonLibs/livoxProto1/CMakeLists.txt @@ -3,9 +3,9 @@ option(ENABLE_LIB_livoxProto1 "Enable Livox Protocol v1 backend lib" OFF) if(ENABLE_LIB_livoxProto1) add_library(livoxProto1 SHARED livoxProto1.cpp - livoxProto1Core.cpp - livoxProto1Device.cpp - livoxProto1Protocol.cpp + core.cpp + device.cpp + protocol.cpp broadcastListener.cpp ) diff --git a/commonLibs/livoxProto1/broadcastListener.cpp b/commonLibs/livoxProto1/broadcastListener.cpp index 8c3fcb7..d652627 100644 --- a/commonLibs/livoxProto1/broadcastListener.cpp +++ b/commonLibs/livoxProto1/broadcastListener.cpp @@ -24,7 +24,8 @@ BroadcastListener::getDevice(const std::string &deviceIdentifier) const { auto it = std::find_if(discoveredDevices.begin(), discoveredDevices.end(), [&deviceIdentifier](const std::shared_ptr& device) { - return device->deviceIdentifier == deviceIdentifier; + return comms::deviceIdentifiersEqual( + device->deviceIdentifier, deviceIdentifier); } ); @@ -43,35 +44,59 @@ void BroadcastListener::broadcastMsgInd( if (bytes_received < sizeof(BroadcastMessage)) { - std::cerr << "Received packet too small: " << bytes_received - << " bytes (expected at least " << sizeof(BroadcastMessage) << ")" - << std::endl; + std::cerr << __func__ + << ": Received packet too small: " << bytes_received + << " bytes (expected at least " + << sizeof(BroadcastMessage) << ")" << std::endl; return; } + // Use placement new to construct BroadcastMessage in the buffer BroadcastMessage* msg = new (bcastMsgRecvBuffer) BroadcastMessage; - // Validate the message using sanity check methods - if (!msg->sanityCheck()) + // Following the clean receiving flow: + // 1. Swap CRC32 to host endianness first + msg->footer.swapCrc32ToHostEndianness(); + // 2. Validate CRC32 (on whole message excluding footer CRC32 field) + if (!msg->validateCrc32()) { - std::cerr << "Broadcast message failed sanity check" << std::endl; + std::cerr << __func__ + << ": Broadcast message failed CRC32 validation" << std::endl; return; } - // Convert from little-endian to host endianness - msg->swapToHostEndianness(); + // 3. Swap CRC16 to host endianness + msg->header.swapCrc16ToHostEndianness(); + // 4. Validate CRC16 (on header only) + if (!msg->header.validateCrc16()) + { + std::cerr << __func__ + << ": Broadcast message failed CRC16 validation" << std::endl; + return; + } + // 5. Swap content to host endianness + msg->swapContentsToHostEndianness(); + // 6. Validate message sanity + if (!msg->sanityCheck()) + { + std::cerr << __func__ + << ": Broadcast message failed sanity check" << std::endl; + return; + } // Extract device information std::string senderIP = senderEndpoint.address().to_string(); - std::string broadcastCode(reinterpret_cast(msg->broadcast_code)); + std::string broadcastCode( + reinterpret_cast(msg->broadcast_code)); // Early return if device already exists if (deviceExists(broadcastCode)) { // Device already exists, just log the update - std::cout << "Received broadcast from known device: " << broadcastCode - << " at " << senderIP << std::endl; + std::cout << __func__ + << ": Received broadcast from known device: " + << broadcastCode << " at " << senderIP << std::endl; return; } @@ -80,8 +105,8 @@ void BroadcastListener::broadcastMsgInd( discoveredDevices.push_back(device); // Output device information using stringify - std::cout << "Discovered new Livox device: " << device->stringify() - << std::endl; + std::cout << __func__ << ": Discovered new Livox device: " + << device->stringify() << std::endl; } void BroadcastListener::start(void) diff --git a/commonLibs/livoxProto1/broadcastListener.h b/commonLibs/livoxProto1/broadcastListener.h index b796117..a3d3329 100644 --- a/commonLibs/livoxProto1/broadcastListener.h +++ b/commonLibs/livoxProto1/broadcastListener.h @@ -6,7 +6,7 @@ #include #include #include -#include "livoxProto1Device.h" +#include "device.h" namespace livoxProto1 { namespace comms { diff --git a/commonLibs/livoxProto1/core.cpp b/commonLibs/livoxProto1/core.cpp new file mode 100644 index 0000000..5e50cba --- /dev/null +++ b/commonLibs/livoxProto1/core.cpp @@ -0,0 +1,136 @@ +#include +#include +#include +#include "protocol.h" +#include "core.h" + +namespace livoxProto1 { + +static ProtoState protoState = +{ + .isInitialized = false, + .componentThread = nullptr, + .deviceManager = nullptr +}; + +ProtoState& getProtoState() +{ + return protoState; +} + +DeviceManager::DeviceManager() +: broadcastListener(protoState.componentThread) +{ + broadcastListener.setDeviceGoneAwayCb(deviceGoneAwayInd); +} + +void DeviceManager::deviceGoneAwayInd(const comms::DiscoveredDevice &device) +{ + std::cout << "Device gone away: " << device.stringify() << std::endl; + + // Check if device exists in our collection + if (!protoState.deviceManager->getDevice(device)) { + return; + } + + // Find and remove the device from the collection + auto it = std::find_if( + protoState.deviceManager->devices.begin(), + protoState.deviceManager->devices.end(), + [&device](const std::shared_ptr &d) { + return d->discoveredDevice == device; + } + ); + if (it != protoState.deviceManager->devices.end()) { + protoState.deviceManager->devices.erase(it); + } +} + +std::shared_ptr DeviceManager::getOrCreateDevice( + const std::string &deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort + ) +{ + // Validate smoIp format using Boost.Asio IPv4 validation + if (!smoIp.empty() && !comms::isValidIPv4(smoIp)) + { + throw std::invalid_argument( + std::string(__func__) + + ": Invalid IPv4 smoIp format: " + smoIp); + } + + // Validate subnet nbits + if (smoSubnetNbits > 32) + { + throw std::invalid_argument( + std::string(__func__) + + ": smoSubnetNbits must be between 0 and 32, got: " + + std::to_string(smoSubnetNbits)); + } + + // First try to get existing device + auto existingDevice = getDevice(deviceIdentifier); + if (existingDevice) { + return existingDevice; + } + + // Device doesn't exist, create a new one + auto newDevice = std::make_shared( + deviceIdentifier, componentThread, + handshakeTimeoutMs, retryDelayMs, + smoIp, smoSubnetNbits, + dataPort, cmdPort, imuPort); + + // Add to our collection + devices.push_back(newDevice); + return newDevice; +} + +std::shared_ptr DeviceManager::getDevice( + const std::string &deviceIdentifier + ) +{ + for (auto& device : devices) + { + if (comms::deviceIdentifiersEqual( + device->discoveredDevice.deviceIdentifier, deviceIdentifier)) + { + return device; + } + } + return nullptr; +} + +bool DeviceManager::isDeviceKnown(const std::string& deviceIdentifier) +{ + return broadcastListener.deviceExists(deviceIdentifier); +} + +void main(const std::shared_ptr &componentThread) +{ + if (protoState.isInitialized) { + return; + } + + protoState.isInitialized = true; + protoState.componentThread = componentThread; + protoState.deviceManager = std::make_unique(); + protoState.deviceManager->broadcastListener.start(); +} + +void exit(void) +{ + if (!protoState.isInitialized) { + return; + } + + protoState.deviceManager->broadcastListener.stop(); + protoState.deviceManager.reset(); + protoState.componentThread.reset(); + protoState.isInitialized = false; +} + +} // namespace livoxProto1 \ No newline at end of file diff --git a/commonLibs/livoxProto1/core.h b/commonLibs/livoxProto1/core.h new file mode 100644 index 0000000..3512e64 --- /dev/null +++ b/commonLibs/livoxProto1/core.h @@ -0,0 +1,60 @@ +#ifndef LIVOXPROTO1_CORE_H +#define LIVOXPROTO1_CORE_H + +#include +#include +#include +#include +#include "device.h" +#include "broadcastListener.h" + +namespace livoxProto1 { + +class DeviceManager +{ +public: + DeviceManager(); + ~DeviceManager() = default; + + static void deviceGoneAwayInd(const comms::DiscoveredDevice &device); + + std::shared_ptr getOrCreateDevice( + const std::string &deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort); + + std::shared_ptr getDevice(const std::string &deviceIdentifier); + std::shared_ptr getDevice(const comms::DiscoveredDevice &device) + { return getDevice(device.deviceIdentifier); } + +private: + // Helper methods + bool isDeviceKnown(const std::string& deviceIdentifier); + + // Configuration + static constexpr int RETRY_DELAY_SECONDS = 3; // seconds delay + +public: + std::vector> devices; + comms::BroadcastListener broadcastListener; +}; + +void main(const std::shared_ptr &componentThread); +void exit(void); + +// Global state structure +struct ProtoState +{ + bool isInitialized = false; + std::shared_ptr componentThread; + std::unique_ptr deviceManager; +}; + +// Access to global state for extern "C" functions +ProtoState& getProtoState(); + +} // namespace livoxProto1 + +#endif // LIVOXPROTO1_CORE_H diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp new file mode 100644 index 0000000..2412286 --- /dev/null +++ b/commonLibs/livoxProto1/device.cpp @@ -0,0 +1,606 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "device.h" +#include "protocol.h" +#include "core.h" + +namespace livoxProto1 { +namespace comms { + +// DiscoveredDevice constructors +DiscoveredDevice::DiscoveredDevice( + const std::string &deviceIdentifier, + DeviceType deviceType, + const std::string &ipAddr) +: deviceIdentifier(deviceIdentifier), + deviceType(deviceType), + ipAddr(ipAddr) +{ +} + +DiscoveredDevice::DiscoveredDevice( + const BroadcastMessage &msg, const std::string &ipAddr +) +: DiscoveredDevice( + reinterpret_cast(msg.broadcast_code), + static_cast(msg.dev_type), + ipAddr) +{ +} + +std::string DiscoveredDevice::stringify(void) const +{ + std::ostringstream oss; + oss << "DiscoveredDevice{" + << "identifier='" << deviceIdentifier << "', " + << "ipAddr='" << ipAddr << "', " + << "deviceType=" << (int)deviceType << " (" << getDeviceTypeName() << ")" + << "}"; + return oss.str(); +} + +std::string DiscoveredDevice::getDeviceTypeName(void) const +{ + switch (deviceType) + { + case DeviceType::Hub: return "Hub"; + case DeviceType::Mid40: return "Mid-40"; + case DeviceType::Tele15: return "Tele-15"; + case DeviceType::Horizon: return "Horizon"; + case DeviceType::Mid70: return "Mid-70"; + case DeviceType::Avia: return "Avia"; + default: return "Unknown"; + } +} + +} // namespace comms + +// Device implementation +Device::Device(const std::string &deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort) +: discoveredDevice( + deviceIdentifier, comms::DeviceType::Mid40, + // Initialize empty. IP will be set upon successful connection. + ""), +componentThread(componentThread), +handshakeTimeoutMs(handshakeTimeoutMs), retryDelayMs(retryDelayMs), +smoIp(smoIp), smoSubnetNbits(smoSubnetNbits), +dataPort(dataPort), cmdPort(cmdPort), imuPort(imuPort), +heartbeatActive(false) +{ + connect(); +} + +Device::~Device() +{ + // Stop heartbeat if active + if (heartbeatActive.load()) { + heartbeatActive.store(false); + if (heartbeatTimer) { + heartbeatTimer->cancel(); + } + } + + // Clean up heartbeat resources + heartbeatTimer.reset(); + heartbeatSocket.reset(); +} + +void Device::connect() +{ + /** EXPLANATION: + * First check the broadcastListener to see if the device is already known. + * * If it is, return the DiscoveredDevice.. + * If it is not, attempt to connect to the device by assuming that its IP + * address is the same as the last 2 octets of the deviceIdentifier. + * * If the connection is successful, return the DiscoveredDevice. + * If the connection is not successful, delay by retryDelayMs and check + * the broadcastListener again. + * * If the connection is successful return the DiscoveredDevice. + * If the connection is not successful, throw exception? + * + * If the connection is successful at any point, also set up the heartbeat + * pulse signal to be sent periodically by us to the device over the wire. + */ + // Try connecting to known device first + if (connectToKnownDevice()) { + startHeartbeat(); + return; + } + + // Try direct connect by device identifier + if (connectByDeviceIdentifier()) { + startHeartbeat(); + return; + } + + // Wait retry delay, then try known device again + std::this_thread::sleep_for(std::chrono::milliseconds(retryDelayMs)); + if (connectToKnownDevice()) { + startHeartbeat(); + return; + } + + // All connection attempts failed + throw std::runtime_error( + std::string(__func__) + ": Failed to connect to device: " + + discoveredDevice.deviceIdentifier); +} + +bool Device::connectToKnownDevice() +{ + // Get the global DeviceManager instance + auto& protoState = livoxProto1::getProtoState(); + if (!protoState.deviceManager) + { + throw std::runtime_error( + std::string(__func__) + + ": DeviceManager is not initialized in connectToKnownDevice()"); + } + + // Check if the device is known to the broadcastListener + if (!protoState.deviceManager->broadcastListener.deviceExists( + discoveredDevice.deviceIdentifier)) + { + return false; + } + + // Get the device info from broadcastListener + auto deviceInfo = protoState.deviceManager->broadcastListener.getDevice( + discoveredDevice.deviceIdentifier); + if (!deviceInfo) + { return false; } + + // Use the IP address from the broadcast message + std::string deviceIP = deviceInfo->ipAddr; + // Execute handshake with the known device + bool success = executeHandshake( + deviceIP, handshakeTimeoutMs, dataPort, cmdPort, imuPort); + + // If successful, update our device's IP address with the one from broadcast + if (success) { + discoveredDevice.ipAddr = deviceInfo->ipAddr; + } + + return success; +} + +bool Device::connectByDeviceIdentifier() +{ + std::string deviceIP = generateClientDeviceIpFromSerialNumber( + discoveredDevice.deviceIdentifier); + bool success = executeHandshake( + deviceIP, handshakeTimeoutMs, dataPort, cmdPort, imuPort); + + // If successful, store the calculated IP address + if (success) { + discoveredDevice.ipAddr = deviceIP; + } + + return success; +} + +bool Device::executeHandshake( + const std::string& deviceIP, int timeoutMs, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort + ) +{ + try { + // Create boost::asio UDP socket + boost::asio::io_context io_context; + boost::asio::ip::udp::socket socket(io_context); + socket.open(boost::asio::ip::udp::v4()); + + std::string smoIp = getSmoIp(); + comms::HandshakeRequest handshakeReq(smoIp, dataPort, cmdPort, imuPort); + handshakeReq.swapContentsToProtocolEndianness(); + handshakeReq.header.setCrc16FromRawBytes(); + handshakeReq.header.swapCrc16ToProtocolEndianness(); + handshakeReq.footer.crc_32 = handshakeReq.calculateCrc32(); + handshakeReq.footer.swapCrc32ToProtocolEndianness(); + boost::asio::ip::udp::endpoint deviceEndpoint( + boost::asio::ip::address::from_string(deviceIP), 65000); + + socket.send_to( + boost::asio::buffer(&handshakeReq, sizeof(handshakeReq)), + deviceEndpoint); + + std::cout << __func__ << ": Sent handshake request to " + << deviceIP << ":65000" << std::endl; + + // Wait for response with timeout using deadline_timer + boost::asio::deadline_timer timer(io_context); + timer.expires_from_now(boost::posix_time::milliseconds(timeoutMs)); + + uint8_t responseBuffer[1024]; + boost::asio::ip::udp::endpoint senderEndpoint; + std::atomic timeoutOccurred{false}; + + timer.async_wait( + [&timeoutOccurred](const boost::system::error_code& ec) { + if (!ec) { timeoutOccurred.store(true); } + } + ); + + size_t bytesReceived = 0; + boost::system::error_code receiveError; + socket.async_receive_from( + boost::asio::buffer(responseBuffer, sizeof(responseBuffer)), + senderEndpoint, + [&bytesReceived, &receiveError]( + const boost::system::error_code& ec, size_t bytes) + { + bytesReceived = bytes; + receiveError = ec; + } + ); + + while (!timeoutOccurred.load() && !receiveError && bytesReceived == 0) { + io_context.run_one(); + } + + timer.cancel(); + + if (timeoutOccurred.load()) + { + std::cerr << __func__ << ": Handshake timeout with " << deviceIP + << ":" << deviceEndpoint << std::endl; + return false; + } + + if (receiveError) + { + std::cerr << __func__ << ": Handshake error with " << deviceIP + << ": " << receiveError.message() << ":" << deviceEndpoint + << std::endl; + return false; + } + + if (bytesReceived < sizeof(comms::HandshakeResponse)) + { + std::cerr << __func__ << ": Handshake failed - response too small from " + << deviceIP << ":" << deviceEndpoint << std::endl; + return false; + } + + // Parse response as complete frame + comms::HandshakeResponse* resp = reinterpret_cast< + comms::HandshakeResponse* + >(responseBuffer); + + // Following the clean receiving flow: + // 1. Swap CRC32 to host endianness first + resp->footer.swapCrc32ToHostEndianness(); + // 2. Validate CRC32 (on whole message excluding footer CRC32 field) + if (!resp->validateCrc32()) + { + std::cerr << __func__ << ": Handshake failed - CRC32 validation " + "failed from " << deviceIP << ":" << deviceEndpoint + << std::endl; + return false; + } + // 3. Swap CRC16 to host endianness + resp->header.swapCrc16ToHostEndianness(); + // 4. Validate CRC16 (on header only) + if (!resp->header.validateCrc16()) + { + std::cerr << __func__ << ": Handshake failed - CRC16 validation " + "failed from " << deviceIP << ":" << deviceEndpoint + << std::endl; + return false; + } + // 5. Swap content to host endianness + resp->swapContentsToHostEndianness(); + if (!resp->sanityCheck() || resp->ret_code != 0x00) + { + std::cerr << __func__ << ": Handshake failed - invalid response from " + << deviceIP << ":" << deviceEndpoint + << std::endl; + return false; + } + + std::cout << __func__ << ": Handshake successful with " << deviceIP + << ":" << deviceEndpoint << std::endl; + return true; + } catch (const std::exception& e) { + std::cerr << __func__ << ": Handshake failed with " << deviceIP << ": " + << e.what() << std::endl; + } + return false; +} + +std::string Device::generateClientDeviceIpFromSerialNumber( + const std::string& broadcastCode + ) +{ + // Determine if input is serial number (14 chars) or broadcast code (15 chars) + if (broadcastCode.empty()) + { + throw std::invalid_argument( + std::string(__func__) + ": Broadcast code cannot be empty"); + } + + std::string serialNumber; + if (broadcastCode.length() == 14) + { + // Input is a serial number + serialNumber = broadcastCode; + } else if (broadcastCode.length() == 15) + { + // Input is a broadcast code (serial + selector) + serialNumber = broadcastCode.substr(0, 14); + } else + { + // Invalid length + throw std::invalid_argument( + std::string(__func__) + + ": Broadcast code must be 14 or 15 characters long"); + } + + // Extract last two digits of serial number + if (serialNumber.length() < 2) + { + throw std::invalid_argument( + std::string(__func__) + ": Serial number too short"); + } + + std::string lastTwoDigits = serialNumber.substr(serialNumber.length() - 2); + // Validate that last two characters are digits + if (lastTwoDigits[0] < '0' || lastTwoDigits[0] > '9' || + lastTwoDigits[1] < '0' || lastTwoDigits[1] > '9') + { + throw std::invalid_argument( + std::string(__func__) + + ": Last two characters of serial number must be digits"); + } + + /** EXPLANATION: + * Use the device's subnet: X.X.X.1XX where XX = last two digits of serial. + * We use the smoIp and smoSubnetNbits to determine the network prefix. + */ + // Parse smoIp to extract network prefix + auto smoIpOctets = comms::parseIPv4Address(smoIp); + if (!smoIpOctets.has_value()) { + throw std::invalid_argument( + std::string(__func__) + ": Invalid smoIp format: must be X.X.X.X"); + } + + // Generate subnet mask based on nbits + uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits); + + // Convert smoIp to uint32_t for bitwise operations + uint32_t smoIpAddr = (std::stoi(smoIpOctets->octet1) << 24) | + (std::stoi(smoIpOctets->octet2) << 16) | + (std::stoi(smoIpOctets->octet3) << 8) | + std::stoi(smoIpOctets->octet4); + + // Apply subnet mask to get network prefix + uint32_t networkPrefix = smoIpAddr & subnetMask; + + // Extract octets from network prefix + uint8_t octet1 = (networkPrefix >> 24) & 0xFF; + uint8_t octet2 = (networkPrefix >> 16) & 0xFF; + uint8_t octet3 = (networkPrefix >> 8) & 0xFF; + + // Use the first three octets and append "1" + last two digits + return std::to_string(octet1) + "." + std::to_string(octet2) + "." + + std::to_string(octet3) + ".1" + lastTwoDigits; +} + +void Device::startHeartbeat() +{ + if (!componentThread || discoveredDevice.ipAddr.empty()) { + return; // Can't start heartbeat without component thread or IP + } + + // Create heartbeat socket using the component thread's io_service + heartbeatSocket = std::make_unique( + componentThread->getIoService()); + heartbeatSocket->open(boost::asio::ip::udp::v4()); + + // Create heartbeat timer + heartbeatTimer = std::make_unique( + componentThread->getIoService()); + + heartbeatActive.store(true); + + // Send first heartbeat immediately + sendHeartbeat(); +} + +void Device::sendHeartbeat() +{ + if (!heartbeatActive.load() || !heartbeatSocket + || discoveredDevice.ipAddr.empty()) + { + return; + } + + try { + // Create heartbeat message using the new HeartbeatMessage type + comms::HeartbeatMessage heartbeatMsg; + + heartbeatMsg.swapContentsToProtocolEndianness(); + heartbeatMsg.header.setCrc16FromRawBytes(); + heartbeatMsg.header.swapCrc16ToProtocolEndianness(); + heartbeatMsg.footer.crc_32 = heartbeatMsg.calculateCrc32(); + heartbeatMsg.footer.swapCrc32ToProtocolEndianness(); + // Send the heartbeat packet + boost::asio::ip::udp::endpoint deviceEndpoint( + boost::asio::ip::address::from_string(discoveredDevice.ipAddr), cmdPort); + + heartbeatSocket->send_to( + boost::asio::buffer(&heartbeatMsg, sizeof(heartbeatMsg)), + deviceEndpoint); + + // Schedule next heartbeat in 1 second + heartbeatTimer->expires_from_now(boost::posix_time::seconds(1)); + heartbeatTimer->async_wait( + [this](const boost::system::error_code& error) { + onHeartbeatTimer(error); + } + ); + } + catch (const std::exception& e) + { + heartbeatActive.store(false); + std::cerr << "[" << __func__ << "] Heartbeat send failed for device " + << discoveredDevice.deviceIdentifier + << ": " << e.what() << std::endl; + } +} + +void Device::onHeartbeatTimer(const boost::system::error_code& error) +{ + // Timer was cancelled, heartbeat stopped + if (error == boost::asio::error::operation_aborted) { + return; + } + + if (error) + { + heartbeatActive.store(false); + std::cerr << "[" << __func__ << "] Heartbeat timer error for device " + << discoveredDevice.deviceIdentifier + << ": " << error.message() << std::endl; + return; + } + + // Send next heartbeat + sendHeartbeat(); +} + +uint32_t Device::getSubnetMaskFor(uint8_t nbits) +{ + if (nbits > 32) { + throw std::invalid_argument( + std::string(__func__) + ": nbits must be between 0 and 32"); + } + + // Generate subnet mask: set the first nbits to 1, rest to 0 + if (nbits == 0) { + return 0x00000000; + } else if (nbits == 32) { + return 0xFFFFFFFF; + } else { + // Create mask with nbits set to 1 from the left + return (0xFFFFFFFF << (32 - nbits)); + } +} + +std::optional Device::detectSmoIp() +{ + try { + // Parse the smoIp to get the network prefix + auto smoIpOctets = comms::parseIPv4Address(smoIp); + if (!smoIpOctets.has_value()) { + return std::nullopt; + } + + // Convert smoIp octets to integers for bitwise operations + uint32_t smoIpAddr = (std::stoi(smoIpOctets->octet1) << 24) | + (std::stoi(smoIpOctets->octet2) << 16) | + (std::stoi(smoIpOctets->octet3) << 8) | + std::stoi(smoIpOctets->octet4); + + // Generate subnet mask based on nbits + uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits); + + // Get all network interfaces using getifaddrs (Linux/Unix specific) + // TODO: Add Windows support using GetAdaptersAddresses when porting + struct ifaddrs *ifaddr; + if (getifaddrs(&ifaddr) == -1) { + return std::nullopt; + } + + // Use unique_ptr for automatic cleanup (RAII) + // This ensures freeifaddrs is called even if we break out of the loop or throw an exception + auto ifaddr_deleter = [](struct ifaddrs* ptr) { freeifaddrs(ptr); }; + std::unique_ptr ifaddr_ptr( + ifaddr, ifaddr_deleter); + + std::string found_ip; + + // Iterate through all network interfaces + for (struct ifaddrs *ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) + { + if (ifa->ifa_addr == nullptr) continue; + + // Check if it's IPv4 + if (ifa->ifa_addr->sa_family != AF_INET) { continue; } + + // Get the IPv4 address + struct sockaddr_in* addr_in = (struct sockaddr_in*)ifa->ifa_addr; + char ip_str[INET_ADDRSTRLEN]; + if (inet_ntop( + AF_INET, &addr_in->sin_addr, ip_str, INET_ADDRSTRLEN) + == nullptr) + { + continue; + } + + std::string ip = ip_str; + + // Check if this IP is in the same subnet + auto ipOctets = comms::parseIPv4Address(ip); + if (!ipOctets.has_value()) { continue; } + + // Convert IP octets to integer + uint32_t ipAddr = (std::stoi(ipOctets->octet1) << 24) | + (std::stoi(ipOctets->octet2) << 16) | + (std::stoi(ipOctets->octet3) << 8) | + std::stoi(ipOctets->octet4); + + // Check if IP matches the subnet using the calculated mask + // Only compare the bits that are set in the subnet mask + if ((ipAddr & subnetMask) == (smoIpAddr & subnetMask)) { + found_ip = ip; + break; // Exit loop, let unique_ptr handle cleanup + } + } + + // Return the found IP (empty string if none found) + if (!found_ip.empty()) { + return found_ip; + } + + return std::nullopt; + } catch (const std::exception& e) { + std::cerr << "Error detecting SMO IP: " << e.what() << std::endl; + return std::nullopt; + } +} + +std::string Device::getSmoIp() +{ + // If smo-ip was provided, return it + if (!smoIp.empty()) { + return smoIp; + } + + // Otherwise, try to detect it + auto detectedIp = detectSmoIp(); + if (detectedIp.has_value()) { + return detectedIp.value(); + } + + // If detection failed, throw an exception + throw std::runtime_error( + std::string(__func__) + ": Failed to detect SMO IP address for smoIp " + + smoIp + " with subnet mask /" + std::to_string(smoSubnetNbits)); +} + +} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/device.h b/commonLibs/livoxProto1/device.h new file mode 100644 index 0000000..02ed376 --- /dev/null +++ b/commonLibs/livoxProto1/device.h @@ -0,0 +1,103 @@ +#ifndef LIVOX_PROTO1_DEVICE_H +#define LIVOX_PROTO1_DEVICE_H + +#include +#include +#include +#include +#include +#include +#include "protocol.h" + +// Forward declaration +namespace smo { + class ComponentThread; +} + +namespace livoxProto1 { +namespace comms { + +/** EXPLANATION: + * This class represents a discovered device. It is used to store the + * device identifier and IP address of a discovered device. + */ +class DiscoveredDevice +{ +public: + DiscoveredDevice( + const std::string &deviceIdentifier, + DeviceType deviceType, + const std::string &ipAddr); + + // "Conversion" constructor from BroadcastMessage + DiscoveredDevice(const BroadcastMessage &msg, const std::string &ipAddr); + + ~DiscoveredDevice() = default; + + bool operator==(const DiscoveredDevice &other) const + { + return comms::deviceIdentifiersEqual( + deviceIdentifier, other.deviceIdentifier); + } + + std::string stringify(void) const; + std::string getDeviceTypeName(void) const; + +public: + std::string deviceIdentifier; + DeviceType deviceType; + std::string ipAddr; +}; + +} // namespace comms + +class Device +{ +public: + Device(const std::string &deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort); + ~Device(); + +public: + comms::DiscoveredDevice discoveredDevice; + + // Configuration + std::shared_ptr componentThread; + int handshakeTimeoutMs, retryDelayMs; + std::string smoIp; + uint8_t smoSubnetNbits; + uint16_t dataPort, cmdPort, imuPort; + +private: + // Connection logic + void connect(); + bool connectToKnownDevice(); + bool connectByDeviceIdentifier(); + bool executeHandshake( + const std::string& deviceIP, int timeoutMs, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort); + std::string generateClientDeviceIpFromSerialNumber( + const std::string& broadcastCode); + + // IP detection methods + std::optional detectSmoIp(); + std::string getSmoIp(); + uint32_t getSubnetMaskFor(uint8_t nbits); + + // Heartbeat mechanism + void startHeartbeat(); + void sendHeartbeat(); + void onHeartbeatTimer(const boost::system::error_code& error); + + // Heartbeat state + std::unique_ptr heartbeatTimer; + std::unique_ptr heartbeatSocket; + std::atomic heartbeatActive; +}; + +} // namespace livoxProto1 + +#endif // LIVOX_PROTO1_DEVICE_H diff --git a/commonLibs/livoxProto1/livoxProto1.cpp b/commonLibs/livoxProto1/livoxProto1.cpp index c87ac18..de5f2da 100644 --- a/commonLibs/livoxProto1/livoxProto1.cpp +++ b/commonLibs/livoxProto1/livoxProto1.cpp @@ -1,14 +1,37 @@ -#include +#include #include "livoxProto1.h" -#include "livoxProto1Core.h" +#include "device.h" +#include "core.h" + extern "C" { -livoxProto1_mainFn livoxProto1_main; -livoxProto1_exitFn livoxProto1_exit; +std::shared_ptr livoxProto1_getOrCreateDevice( + const std::string& deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort +) +{ + // Get the global DeviceManager instance + auto& protoState = livoxProto1::getProtoState(); + if (!protoState.deviceManager) + { + throw std::runtime_error( + std::string(__func__) + ": LivoxProto1 not initialized - call " + "livoxProto1_main first"); + } -void livoxProto1_main( - const std::shared_ptr &componentThread) + // Delegate to DeviceManager + return protoState.deviceManager->getOrCreateDevice( + deviceIdentifier, componentThread, + handshakeTimeoutMs, retryDelayMs, + smoIp, smoSubnetNbits, + dataPort, cmdPort, imuPort); +} + +void livoxProto1_main(const std::shared_ptr& componentThread) { livoxProto1::main(componentThread); } diff --git a/commonLibs/livoxProto1/livoxProto1.h b/commonLibs/livoxProto1/livoxProto1.h index a2f85ff..32ca282 100644 --- a/commonLibs/livoxProto1/livoxProto1.h +++ b/commonLibs/livoxProto1/livoxProto1.h @@ -1,25 +1,61 @@ -#ifndef LIVOX_PROTO1_API_H -#define LIVOX_PROTO1_API_H +#ifndef LIVOXPROTO1_H +#define LIVOXPROTO1_H -#include -#include #include -#include -#include "livoxProto1Core.h" -#include "livoxProto1Device.h" +#include +#include + +// Forward declarations +namespace smo { + class ComponentThread; +} namespace livoxProto1 { + class Device; +} -} // namespace livoxProto1 - +#ifdef __cplusplus extern "C" { - typedef void (livoxProto1_mainFn)( - const std::shared_ptr &componentThread); - typedef void (livoxProto1_exitFn)(void); +#endif - void livoxProto1_main( - const std::shared_ptr &componentThread); - void livoxProto1_exit(void); -} // extern "C" +/** + * Initialize the Livox protocol library + * @param componentThread Component thread shared pointer + */ +typedef void livoxProto1_mainFn( + const std::shared_ptr& componentThread); -#endif // LIVOX_PROTO1_API_H +/** + * Cleanup the Livox protocol library + */ +typedef void livoxProto1_exitFn(void); + +/** + * Create a new Livox device connection + * @param deviceIdentifier The device identifier (broadcast code) + * @param componentThread Component thread for async operations + * @param handshakeTimeoutMs Handshake timeout in milliseconds (default: 1000) + * @param retryDelayMs Retry delay in milliseconds (default: 3000) + * @param smoIp SMO IP address (empty string for auto-detection) + * @param smoSubnetNbits SMO subnet mask bits (e.g., 24 for /24, 16 for /16) + * @param dataPort Data port for point cloud (default: 56000) + * @param cmdPort Command port (default: 56001) + * @param imuPort IMU port (default: 56002) + * @return Device pointer on success, nullptr on failure + */ +typedef std::shared_ptr livoxProto1_getOrCreateDeviceFn( + const std::string& deviceIdentifier, + const std::shared_ptr& componentThread, + int handshakeTimeoutMs, int retryDelayMs, + const std::string& smoIp, uint8_t smoSubnetNbits, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort); + +livoxProto1_mainFn livoxProto1_main; +livoxProto1_exitFn livoxProto1_exit; +livoxProto1_getOrCreateDeviceFn livoxProto1_getOrCreateDevice; + +#ifdef __cplusplus +} +#endif + +#endif // LIVOXPROTO1_H \ No newline at end of file diff --git a/commonLibs/livoxProto1/livoxProto1Core.cpp b/commonLibs/livoxProto1/livoxProto1Core.cpp deleted file mode 100644 index 4cb48df..0000000 --- a/commonLibs/livoxProto1/livoxProto1Core.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include -#include "livoxProto1Protocol.h" -#include "livoxProto1Core.h" - -namespace livoxProto1 { - -struct ProtoState -{ - bool isInitialized = false; - std::shared_ptr componentThread; - std::unique_ptr deviceManager; -}; - -static ProtoState protoState = -{ - .isInitialized = false, - .componentThread = nullptr, - .deviceManager = nullptr -}; - -DeviceManager::DeviceManager() -: broadcastListener(protoState.componentThread) -{ - broadcastListener.setDeviceGoneAwayCb(deviceGoneAwayInd); -} - -void DeviceManager::deviceGoneAwayInd(const comms::DiscoveredDevice &device) -{ - std::cout << "Device gone away: " << device.stringify() << std::endl; - auto it = std::find_if( - protoState.deviceManager->devices.begin(), - protoState.deviceManager->devices.end(), - [&device](const Device &d) { - return d.discoveredDevice == device; - } - ); - - if (it != protoState.deviceManager->devices.end()) { - protoState.deviceManager->devices.erase(it); - } -} - -void main(const std::shared_ptr &componentThread) -{ - if (protoState.isInitialized) { - return; - } - - protoState.isInitialized = true; - protoState.componentThread = componentThread; - protoState.deviceManager = std::make_unique(); - protoState.deviceManager->broadcastListener.start(); -} - -void exit(void) -{ - if (!protoState.isInitialized) { - return; - } - - protoState.deviceManager->broadcastListener.stop(); - protoState.deviceManager.reset(); - protoState.isInitialized = false; - protoState.componentThread = nullptr; -} - -} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/livoxProto1Core.h b/commonLibs/livoxProto1/livoxProto1Core.h deleted file mode 100644 index acd648c..0000000 --- a/commonLibs/livoxProto1/livoxProto1Core.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef LIVOXPROTO1_CORE_H -#define LIVOXPROTO1_CORE_H - -#include -#include -#include -#include "livoxProto1Device.h" -#include "broadcastListener.h" - -namespace livoxProto1 { - -class DeviceManager -{ -public: - DeviceManager(); - ~DeviceManager() = default; - - static void deviceGoneAwayInd(const comms::DiscoveredDevice &device); - -public: - std::vector devices; - comms::BroadcastListener broadcastListener; -}; - -void main(const std::shared_ptr &componentThread); -void exit(void); - -} // namespace livoxProto1 - -#endif // LIVOXPROTO1_CORE_H diff --git a/commonLibs/livoxProto1/livoxProto1Device.cpp b/commonLibs/livoxProto1/livoxProto1Device.cpp deleted file mode 100644 index c670707..0000000 --- a/commonLibs/livoxProto1/livoxProto1Device.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include -#include "livoxProto1Device.h" - -namespace livoxProto1 { -namespace comms { - -// DiscoveredDevice constructors -DiscoveredDevice::DiscoveredDevice( - const std::string &deviceIdentifier, - DeviceType deviceType, - const std::string &ipAddr) -: deviceIdentifier(deviceIdentifier), - deviceType(deviceType), - ipAddr(ipAddr) -{ -} - -DiscoveredDevice::DiscoveredDevice( - const BroadcastMessage &msg, const std::string &ipAddr -) -: DiscoveredDevice( - reinterpret_cast(msg.broadcast_code), - static_cast(msg.dev_type), - ipAddr) -{ -} - -std::string DiscoveredDevice::stringify(void) const -{ - std::ostringstream oss; - oss << "DiscoveredDevice{" - << "identifier='" << deviceIdentifier << "', " - << "ipAddr='" << ipAddr << "', " - << "deviceType=" << (int)deviceType << " (" << getDeviceTypeName() << ")" - << "}"; - return oss.str(); -} - -std::string DiscoveredDevice::getDeviceTypeName(void) const -{ - switch (deviceType) - { - case DeviceType::Hub: return "Hub"; - case DeviceType::Mid40: return "Mid-40"; - case DeviceType::Tele15: return "Tele-15"; - case DeviceType::Horizon: return "Horizon"; - case DeviceType::Mid70: return "Mid-70"; - case DeviceType::Avia: return "Avia"; - default: return "Unknown"; - } -} - -} // namespace comms - -// Device implementation -Device::Device(const comms::DiscoveredDevice &discoveredDevice) -: discoveredDevice(discoveredDevice) -{ -} - -} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/livoxProto1Device.h b/commonLibs/livoxProto1/livoxProto1Device.h deleted file mode 100644 index 3801bd6..0000000 --- a/commonLibs/livoxProto1/livoxProto1Device.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef LIVOX_PROTO1_DEVICE_H -#define LIVOX_PROTO1_DEVICE_H - -#include -#include "livoxProto1Protocol.h" - -namespace livoxProto1 { -namespace comms { - -/** EXPLANATION: - * This class represents a discovered device. It is used to store the - * device identifier and IP address of a discovered device. - */ -class DiscoveredDevice -{ -public: - DiscoveredDevice( - const std::string &deviceIdentifier, - DeviceType deviceType, - const std::string &ipAddr); - - // "Conversion" constructor from BroadcastMessage - DiscoveredDevice(const BroadcastMessage &msg, const std::string &ipAddr); - - ~DiscoveredDevice() = default; - - bool operator==(const DiscoveredDevice &other) const - { return deviceIdentifier == other.deviceIdentifier; } - - std::string stringify(void) const; - std::string getDeviceTypeName(void) const; - -public: - std::string deviceIdentifier; - DeviceType deviceType; - std::string ipAddr; -}; - -} // namespace comms - -class Device -{ -public: - Device(const comms::DiscoveredDevice &discoveredDevice); - ~Device() = default; - -public: - comms::DiscoveredDevice discoveredDevice; -}; - -} // namespace livoxProto1 - -#endif // LIVOX_PROTO1_DEVICE_H diff --git a/commonLibs/livoxProto1/livoxProto1Protocol.cpp b/commonLibs/livoxProto1/livoxProto1Protocol.cpp deleted file mode 100644 index f2c7483..0000000 --- a/commonLibs/livoxProto1/livoxProto1Protocol.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include "livoxProto1Protocol.h" - -namespace livoxProto1 { -namespace comms { - -// Header methods -void Header::swapToHostEndianness() -{ - if (endian::isLittleEndian()) { return; } - length = __builtin_bswap16(length); - seq_num = __builtin_bswap16(seq_num); - crc_16 = __builtin_bswap16(crc_16); -} - -bool Header::sanityCheck() const -{ - return (sof == 0xAA) && (version == 1); -} - -// Footer methods -void Footer::swapToHostEndianness() -{ - if (endian::isLittleEndian()) { return; } - crc_32 = __builtin_bswap32(crc_32); -} - -bool Footer::sanityCheck() const -{ - /** FIXME: - * Add CRC validation here. - */ - return true; -} - -// BroadcastMessage methods -void BroadcastMessage::swapToHostEndianness() -{ - if (endian::isLittleEndian()) { return; } - header.swapToHostEndianness(); - reserved = __builtin_bswap16(reserved); - footer.swapToHostEndianness(); -} - -bool BroadcastMessage::sanityCheck() const -{ - return header.sanityCheck() && - (cmd_set == 0x00) && - (cmd_id == 0x00) && - (header.cmd_type == 0x02) && - footer.sanityCheck(); -} - -} // namespace comms -} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/livoxProto1Protocol.h b/commonLibs/livoxProto1/livoxProto1Protocol.h deleted file mode 100644 index 3fe5bce..0000000 --- a/commonLibs/livoxProto1/livoxProto1Protocol.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef LIVOXPROTO1_PROTOCOL_H -#define LIVOXPROTO1_PROTOCOL_H - -#include -#include -#include -#include -#include -#include - -namespace livoxProto1 { -namespace comms { - -// Endianness detection -namespace endian { - inline bool isLittleEndian() { - union { - uint32_t i; - char c[4]; - } test = {0x01020304}; - return test.c[0] == 4; - } -} - -/** 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(); - bool sanityCheck() const; -} __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(); - 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 - uint8_t cmd_set; // 9: Command Set (0x00 = General) - uint8_t cmd_id; // 10: Command ID (0x00 = Broadcast) - 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 swapToHostEndianness(); - bool sanityCheck() const; -} __attribute__((packed)); - -} // namespace comms -} // namespace livoxProto1 - -#endif // LIVOXPROTO1_PROTOCOL_H diff --git a/commonLibs/livoxProto1/protocol.cpp b/commonLibs/livoxProto1/protocol.cpp new file mode 100644 index 0000000..3f10d59 --- /dev/null +++ b/commonLibs/livoxProto1/protocol.cpp @@ -0,0 +1,626 @@ +#include +#include +#include +#include +#include "protocol.h" + +namespace livoxProto1 { +namespace comms { + +// Command methods +void Command::swapToHostEndianness() +{ + // No multi-byte fields to swap +} + +void Command::swapToProtocolEndianness() +{ + // No multi-byte fields to swap +} + +bool Command::sanityCheck() const +{ + // Basic validation - can be extended for specific command sets + return true; +} + +// Header methods +void Header::swapToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + length = __builtin_bswap16(length); + seq_num = __builtin_bswap16(seq_num); + crc_16 = __builtin_bswap16(crc_16); +} + +void Header::swapToProtocolEndianness() +{ + // Protocol is little-endian, so if host is already little-endian, no swap needed + if (endian::isLittleEndian()) { return; } + // Host is big-endian, need to swap to little-endian + length = __builtin_bswap16(length); + seq_num = __builtin_bswap16(seq_num); + crc_16 = __builtin_bswap16(crc_16); +} + +bool Header::sanityCheck() const +{ + return (sof == 0xAA) && (version == 1); +} + +uint16_t Header::calculateCrc16() const +{ + // Calculate CRC16 for the header excluding the crc_16 field itself + // This matches the Livox SDK approach: calculate over raw bytes excluding CRC16 field + const uint8_t* headerData = reinterpret_cast(this); + size_t headerSize = sizeof(Header) - sizeof(crc_16); // Exclude CRC16 field + + return comms::calculateCrc16(headerData, headerSize); +} + +bool Header::validateCrc16() const +{ + // Calculate CRC16 for the header excluding the crc_16 field itself + uint16_t calculatedCrc = calculateCrc16(); + + // Debug output + std::cout << "CRC16 Debug: calculated=0x" << std::hex << calculatedCrc + << ", received=0x" << crc_16 << std::dec << std::endl; + + // Compare with the CRC in the header + return calculatedCrc == crc_16; +} + +void Header::setCrc16FromRawBytes() +{ + // Calculate CRC16 on raw bytes and set it (after endianness swap) + crc_16 = calculateCrc16(); +} + +void Header::swapCrc16ToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + crc_16 = __builtin_bswap16(crc_16); +} + +void Header::swapCrc16ToProtocolEndianness() +{ + if (endian::isLittleEndian()) { return; } + crc_16 = __builtin_bswap16(crc_16); +} + +// Footer methods +void Footer::swapToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + crc_32 = __builtin_bswap32(crc_32); +} + +void Footer::swapToProtocolEndianness() +{ + // Protocol is little-endian, so if host is already little-endian, no swap needed + if (endian::isLittleEndian()) { return; } + // Host is big-endian, need to swap to little-endian + crc_32 = __builtin_bswap32(crc_32); +} + +void Footer::swapCrc32ToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + crc_32 = __builtin_bswap32(crc_32); +} + +void Footer::swapCrc32ToProtocolEndianness() +{ + if (endian::isLittleEndian()) { return; } + crc_32 = __builtin_bswap32(crc_32); +} + +bool Footer::validateCrc32() const +{ + // This method should validate the CRC32 against the message content + // For now, we'll return true since the validation is done on raw bytes + // before struct construction in the receiving flow + return true; +} + +bool Footer::sanityCheck() const +{ + /** FIXME: + * Add CRC validation here. + */ + return true; +} + +// BroadcastMessage methods +void BroadcastMessage::swapContentsToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + // Only swap content fields, not CRC fields + header.swapToHostEndianness(); + command.swapToHostEndianness(); + reserved = __builtin_bswap16(reserved); + // Note: footer.swapToHostEndianness() swaps CRC, so we skip it here +} + + +bool BroadcastMessage::sanityCheck() const +{ + return header.sanityCheck() && + command.sanityCheck() && + (command.cmd_set == 0x00) && + (command.cmd_id == 0x00) && + (header.cmd_type == 0x02) && + footer.sanityCheck(); +} + +bool BroadcastMessage::validateCrc32() const +{ + // Calculate CRC32 for the entire message excluding the footer.crc_32 field + // Try calculating on the raw bytes of the entire message (excluding CRC field) + uint32_t calculatedCrc = 0xFFFFFFFF; + + // Calculate CRC32 over the entire message except the CRC field itself + // The message structure is: header + command + broadcast_code + dev_type + reserved + footer(without crc_32) + const uint8_t* messageData = reinterpret_cast(this); + size_t messageSize = sizeof(BroadcastMessage) - sizeof(footer.crc_32); + + calculatedCrc = comms::calculateCrc32(messageData, messageSize); + + // Debug output + std::cout << "BroadcastMessage CRC32 Debug: calculated=0x" << std::hex << calculatedCrc + << ", received=0x" << footer.crc_32 << std::dec << std::endl; + + // Compare with the CRC in the footer + return calculatedCrc == footer.crc_32; +} + +// HandshakeRequest methods +HandshakeRequest::HandshakeRequest( + const std::string& hostIP, + uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort + ) +{ + // Initialize header + header.sof = 0xAA; + header.version = 1; + header.length = sizeof(HandshakeRequest); + header.cmd_type = 0x00; // CMD (request) + header.seq_num = 1; // Sequence number + header.crc_16 = 0; // Will be calculated later + + // Initialize command + command.cmd_set = 0x00; // General Command Set + command.cmd_id = 0x01; // Handshake Command + + // Parse host IP address + std::istringstream iss(hostIP); + std::string token; + int i = 0; + while (std::getline(iss, token, '.') && i < 4) + { + user_ip[i] = static_cast(std::stoi(token)); + i++; + } + + // Set ports + this->data_port = dataPort; + this->cmd_port = cmdPort; + this->imu_port = imuPort; + + // Initialize footer + footer.crc_32 = 0; // Will be calculated later + // Note: CRC16 will be calculated before sending (in swapToProtocolEndianness) +} + +uint32_t HandshakeRequest::calculateCrc32() const +{ + // Calculate CRC32 for the entire message excluding the footer.crc_32 field + const uint8_t* messageData = reinterpret_cast(this); + size_t messageSize = sizeof(HandshakeRequest) - sizeof(footer.crc_32); + + return comms::calculateCrc32(messageData, messageSize); +} + + +void HandshakeRequest::swapContentsToProtocolEndianness() +{ + // Protocol uses little-endian, so on little-endian machines, no swap needed + if (endian::isLittleEndian()) { return; } + + // On big-endian machines, swap to little-endian for wire transmission + // Only swap content fields, not CRC fields + header.swapToHostEndianness(); + command.swapToHostEndianness(); + data_port = __builtin_bswap16(data_port); + cmd_port = __builtin_bswap16(cmd_port); + imu_port = __builtin_bswap16(imu_port); + // Note: footer.swapToHostEndianness() swaps CRC, so we skip it here +} + +void HandshakeRequest::swapContentsToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + header.swapToHostEndianness(); + command.swapToHostEndianness(); + data_port = __builtin_bswap16(data_port); + cmd_port = __builtin_bswap16(cmd_port); + imu_port = __builtin_bswap16(imu_port); + // Note: footer.swapToHostEndianness() swaps CRC, so we skip it here +} + + +bool HandshakeRequest::sanityCheck() const +{ + return header.sanityCheck() && + command.sanityCheck() && + (command.cmd_set == 0x00) && (command.cmd_id == 0x01) && + footer.sanityCheck(); +} + +// HandshakeResponse methods +void HandshakeResponse::swapContentsToHostEndianness() +{ + if (endian::isLittleEndian()) { return; } + // Only swap content fields, not CRC fields + header.swapToHostEndianness(); + command.swapToHostEndianness(); + // Note: footer.swapToHostEndianness() swaps CRC, so we skip it here +} + + +bool HandshakeResponse::sanityCheck() const +{ + return header.sanityCheck() && + command.sanityCheck() && + (command.cmd_set == 0x00) && (command.cmd_id == 0x01) && + footer.sanityCheck(); +} + +bool HandshakeResponse::validateCrc32() const +{ + // Calculate CRC32 for the entire message excluding the footer.crc_32 field + const uint8_t* messageData = reinterpret_cast(this); + size_t messageSize = sizeof(HandshakeResponse) - sizeof(footer.crc_32); + uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize); + + // Debug output + std::cout << "HandshakeResponse CRC32 Debug: calculated=0x" << std::hex << calculatedCrc + << ", received=0x" << footer.crc_32 << std::dec << std::endl; + + // Compare with the CRC in the footer + return calculatedCrc == footer.crc_32; +} + +// Standalone CRC16 calculation utility +uint16_t calculateCrc16(const uint8_t* data, size_t length) +{ + /** EXPLANATION: + * Livox SDK CRC16 implementation (exact copy from FastCRC library) + * This matches the exact implementation used by Livox devices + */ + static const uint16_t crc_table_mcrf4xx[1024] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 + }; + + // Livox SDK seed + uint16_t crc = LIVOX_CRC16_SEED; + + // Simple implementation for now - can be optimized later + for (size_t i = 0; i < length; ++i) { + crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ data[i]]; + } + + return crc; +} + +// Standalone CRC32 calculation utility +uint32_t calculateCrc32(const uint8_t* data, size_t length) +{ + /** EXPLANATION: + * Livox SDK CRC32 implementation (exact copy from FastCRC library) + * This matches the exact implementation used by Livox devices + */ + static const uint32_t crc_table_crc32[256] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, + 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, + 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, + 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, + 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, + 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, + 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, + 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, + 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, + 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, + 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, + 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, + 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, + 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, + 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, + 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, + 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, + 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, + 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, + 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, + 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, + 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, + 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, + 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, + 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, + 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, + 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, + 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, + 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, + 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, + 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, + 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, + 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + }; + + // Livox SDK seed XORed with 0xffffffff + uint32_t crc = LIVOX_CRC32_SEED ^ 0xffffffff; + + for (size_t i = 0; i < length; ++i) { + crc = (crc >> 8) ^ crc_table_crc32[(crc & 0xff) ^ data[i]]; + } + + return crc ^ 0xffffffff; +} + +// IP address parsing utility +std::optional parseIPv4Address(const std::string& ipAddress) +{ + IPOctets result; + + std::istringstream iss(ipAddress); + if (std::getline(iss, result.octet1, '.') && + std::getline(iss, result.octet2, '.') && + std::getline(iss, result.octet3, '.') && + std::getline(iss, result.octet4, '.')) + { + return result; + } + + return std::nullopt; +} + +// HeartbeatMessage methods +HeartbeatMessage::HeartbeatMessage() +{ + // Initialize header + header.sof = 0xAA; + header.version = 0x01; + header.length = sizeof(Header) + sizeof(Command) + sizeof(Footer); + header.cmd_type = 0x00; // kCommandTypeCmd + header.seq_num = 0x0001; // Simple sequence number + header.crc_16 = 0; // Will be calculated + + // Initialize command + command.cmd_set = 0x00; // kCommandSetGeneral + command.cmd_id = 0x03; // kCommandIDGeneralHeartbeat + + // Initialize footer + footer.crc_32 = 0; // Will be calculated + // Note: CRC16 will be calculated before sending (in swapToProtocolEndianness) +} + +uint32_t HeartbeatMessage::calculateCrc32() const +{ + // Calculate CRC32 for the entire message excluding the footer.crc_32 field + const uint8_t* messageData = reinterpret_cast(this); + size_t messageSize = sizeof(HeartbeatMessage) - sizeof(footer.crc_32); + + return comms::calculateCrc32(messageData, messageSize); +} + + +void HeartbeatMessage::swapContentsToProtocolEndianness() +{ + // Protocol is little-endian, so if host is already little-endian, no swap needed + if (endian::isLittleEndian()) { + return; + } + + // Host is big-endian, need to swap to little-endian + // Only swap content fields, not CRC fields + header.swapToProtocolEndianness(); + command.swapToProtocolEndianness(); + // Note: footer.swapToProtocolEndianness() swaps CRC, so we skip it here +} + +void HeartbeatMessage::swapContentsToHostEndianness() +{ + // If host is already little-endian, no swap needed + if (endian::isLittleEndian()) { + return; + } + + // Host is big-endian, need to swap from little-endian protocol to big-endian host + // Only swap content fields, not CRC fields + header.swapToHostEndianness(); + command.swapToHostEndianness(); + // Note: footer.swapToHostEndianness() swaps CRC, so we skip it here +} + + +bool HeartbeatMessage::sanityCheck() const +{ + return header.sanityCheck() && + command.sanityCheck() && + (command.cmd_set == 0x00) && (command.cmd_id == 0x03) && + footer.sanityCheck(); +} + +bool HeartbeatMessage::validateCrc32() const +{ + // Use the calculateCrc32 method to avoid code duplication + uint32_t calculatedCrc = calculateCrc32(); + + // Debug output + std::cout << "HeartbeatMessage CRC32 Debug: calculated=0x" << std::hex << calculatedCrc + << ", received=0x" << footer.crc_32 << std::dec << std::endl; + + // Compare with the CRC in the footer + return calculatedCrc == footer.crc_32; +} + +} // namespace comms +} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/protocol.h b/commonLibs/livoxProto1/protocol.h new file mode 100644 index 0000000..0e9ea73 --- /dev/null +++ b/commonLibs/livoxProto1/protocol.h @@ -0,0 +1,232 @@ +#ifndef LIVOXPROTO1_PROTOCOL_H +#define LIVOXPROTO1_PROTOCOL_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace livoxProto1 { +namespace comms { + +// Livox SDK CRC constants +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 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(); + void swapContentsToHostEndianness(); + bool sanityCheck() const; +} __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(); + void swapContentsToHostEndianness(); + bool sanityCheck() const; + bool validateCrc32() const; +} __attribute__((packed)); + +} // namespace comms +} // namespace livoxProto1 + +#endif // LIVOXPROTO1_PROTOCOL_H diff --git a/docs/livox-gen1-lidar-dap-spec.md b/docs/livox-gen1-lidar-dap-spec.md index 3a5bc74..4f18000 100644 --- a/docs/livox-gen1-lidar-dap-spec.md +++ b/docs/livox-gen1-lidar-dap-spec.md @@ -16,13 +16,13 @@ The LivoxGen1Lidar DAP uses a unified API name `livoxGen1` with mode-based param **Syntax**: ``` -+idev | avia0 | structural-implexor | livoxGen1(mode=pointCloudIntensity) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 ++idev | avia0 | structural-implexor | livoxGen1(mode=pointCloudIntensity) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ``` **Alternative Syntax** (all equivalent): ``` -+idev | avia0 | structural-implexor | livoxGen1(stim=pcloudIntensity) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 -+idev | avia0 | structural-implexor | livoxGen1(affordance=pCloudI) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 ++idev | avia0 | structural-implexor | livoxGen1(stim=pcloudIntensity) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ++idev | avia0 | structural-implexor | livoxGen1(affordance=pCloudI) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ``` **Mode Parameter Values** (synonymous): @@ -38,7 +38,7 @@ The LivoxGen1Lidar DAP uses a unified API name `livoxGen1` with mode-based param **Syntax**: ``` -+edev | avia0 | structural-implexor | livoxGen1(mode=pcloud,format=xyz) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 ++edev | avia0 | structural-implexor | livoxGen1(mode=pcloud,format=xyz) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ``` **Mode Parameter Values** (synonymous): @@ -62,7 +62,7 @@ The LivoxGen1Lidar DAP uses a unified API name `livoxGen1` with mode-based param **Syntax**: ``` -+idev | avia0 | gyro-implexor | livoxGen1(mode=gyro) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 ++idev | avia0 | gyro-implexor | livoxGen1(mode=gyro) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ``` **Mode Parameter Values**: @@ -74,7 +74,7 @@ The LivoxGen1Lidar DAP uses a unified API name `livoxGen1` with mode-based param **Syntax**: ``` -+idev | avia0 | accel-implexor | livoxGen1(mode=accel) | livoxProto1(retry-delay-ms=3000) | 3JEDK380010Z39 ++idev | avia0 | accel-implexor | livoxGen1(mode=accel) | livoxProto1(handshake-timeout-ms=1000,retry-delay-ms=3000,smo-ip=192.168.1.50,smo-subnet-nbits=24) | 3JEDK380010Z39 ``` **Mode Parameter Values**: @@ -86,12 +86,24 @@ The LivoxGen1Lidar DAP uses a unified API name `livoxGen1` with mode-based param The `livoxProto1` provider accepts the following parameters: +**handshake-timeout-ms** (optional): +- Specifies the timeout for handshake operations when connecting to devices +- Value: Integer number of milliseconds +- Example: `handshake-timeout-ms=1000` (1 second timeout) +- Default: 1000ms if not specified + **retry-delay-ms** (optional): - Specifies how long to wait for broadcast messages to arrive after attempting an initial direct connection - Value: Integer number of milliseconds - Example: `retry-delay-ms=3000` (wait 3 seconds) - Default: 3000ms if not specified +**subnet** (optional): +- Specifies the IP subnet for device IP address calculation +- Value: IP address in the form X.X.0.0 where non-subnet bits must be 0 +- Example: `subnet=10.42.0.0` (use 10.42.x.x subnet) +- Default: 0.0.0.0 (use default 192.168.1.x subnet) + **data-port** (optional): - Specifies the UDP port for receiving point cloud data from the device - Value: Integer port number