Livox: Add new loadable libs for Protocolv1 and gen1 lidars

We've added two new libs:
* commonLibs/livoxProto1
* senseApis/livoxGen1

They currently get to the point of detecting my Livox Avia on the
network over UDP. This was really easy to get done in one night
using boost::asio and Cursor, honestly.
This commit is contained in:
2025-09-05 00:08:25 -04:00
parent f00e1c7cf1
commit 6eb6fa1eb0
13 changed files with 771 additions and 0 deletions
+1
View File
@@ -1 +1,2 @@
add_subdirectory(xcbXorg)
add_subdirectory(livoxProto1)
+18
View File
@@ -0,0 +1,18 @@
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
)
# Set config define for header generation
add_compile_definitions(CONFIG_LIB_LIVOXPROTO1_ENABLED)
target_include_directories(livoxProto1 PUBLIC ${Boost_INCLUDE_DIRS})
target_link_libraries(livoxProto1 ${Boost_LIBRARIES})
# Install rules
install(TARGETS livoxProto1 DESTINATION lib)
endif()
+21
View File
@@ -0,0 +1,21 @@
#include <user/senseApiDesc.h>
#include "livoxProto1.h"
#include "livoxProto1Core.h"
extern "C" {
livoxProto1_mainFn livoxProto1_main;
livoxProto1_exitFn livoxProto1_exit;
void livoxProto1_main(
const std::shared_ptr<smo::ComponentThread> &componentThread)
{
livoxProto1::main(componentThread);
}
void livoxProto1_exit(void)
{
livoxProto1::exit();
}
} // extern "C"
+25
View File
@@ -0,0 +1,25 @@
#ifndef LIVOX_PROTO1_API_H
#define LIVOX_PROTO1_API_H
#include <string>
#include <vector>
#include <memory>
#include <map>
#include "livoxProto1Core.h"
#include "livoxProto1Device.h"
namespace livoxProto1 {
} // namespace livoxProto1
extern "C" {
typedef void (livoxProto1_mainFn)(
const std::shared_ptr<smo::ComponentThread> &componentThread);
typedef void (livoxProto1_exitFn)(void);
void livoxProto1_main(
const std::shared_ptr<smo::ComponentThread> &componentThread);
void livoxProto1_exit(void);
} // extern "C"
#endif // LIVOX_PROTO1_API_H
@@ -0,0 +1,67 @@
#include <user/senseApiDesc.h>
#include "livoxProto1Protocol.h"
#include "livoxProto1Core.h"
namespace livoxProto1 {
struct ProtoState
{
bool isInitialized = false;
std::shared_ptr<smo::ComponentThread> componentThread;
std::unique_ptr<DeviceManager> 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<smo::ComponentThread> &componentThread)
{
if (protoState.isInitialized) {
return;
}
protoState.isInitialized = true;
protoState.componentThread = componentThread;
protoState.deviceManager = std::make_unique<DeviceManager>();
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
+39
View File
@@ -0,0 +1,39 @@
#ifndef LIVOXPROTO1_CORE_H
#define LIVOXPROTO1_CORE_H
#include <vector>
#include <string>
#include <memory>
#include "livoxProto1Protocol.h"
namespace livoxProto1 {
class Device
{
public:
Device(const comms::DiscoveredDevice &discoveredDevice);
~Device() = default;
public:
comms::DiscoveredDevice discoveredDevice;
};
class DeviceManager
{
public:
DeviceManager();
~DeviceManager() = default;
static void deviceGoneAwayInd(const comms::DiscoveredDevice &device);
public:
std::vector<Device> devices;
comms::BroadcastListener broadcastListener;
};
void main(const std::shared_ptr<smo::ComponentThread> &componentThread);
void exit(void);
} // namespace livoxProto1
#endif // LIVOXPROTO1_CORE_H
@@ -0,0 +1,243 @@
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cstring>
#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();
}
// 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<const char*>(msg.broadcast_code),
static_cast<DeviceType>(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";
}
}
BroadcastListener::BroadcastListener(
const std::shared_ptr<smo::ComponentThread>& componentThread,
uint16_t listeningPort, uint16_t connectPort
)
: componentThread(componentThread),
listeningPort(listeningPort),
connectPort(connectPort),
deviceGoneAwayCb(nullptr),
socket(componentThread->getIoService()),
listeningEndpoint(boost::asio::ip::udp::v4(), listeningPort),
isListening(false)
{
}
std::shared_ptr<DiscoveredDevice>
BroadcastListener::getDevice(const std::string &deviceIdentifier) const
{
auto it = std::find_if(discoveredDevices.begin(), discoveredDevices.end(),
[&deviceIdentifier](const std::shared_ptr<DiscoveredDevice>& device) {
return device->deviceIdentifier == deviceIdentifier;
}
);
return it != discoveredDevices.end() ? *it : nullptr;
}
void BroadcastListener::broadcastMsgInd(
const boost::system::error_code& ec, std::size_t bytes_received)
{
if (ec)
{
std::cerr << __func__ << ": Error receiving broadcast message: "
<< ec.message() << std::endl;
return;
}
if (bytes_received < sizeof(BroadcastMessage))
{
std::cerr << "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;
if (!msg->sanityCheck())
{
std::cerr << "Broadcast message failed sanity check" << std::endl;
return;
}
// Convert from little-endian to host endianness
msg->swapToHostEndianness();
// Extract device information
std::string senderIP = senderEndpoint.address().to_string();
std::string broadcastCode(reinterpret_cast<const char*>(msg->broadcast_code));
// Early return if device already exists
if (deviceExists(broadcastCode)) { return; }
// Create new DiscoveredDevice using conversion constructor
auto device = std::make_shared<DiscoveredDevice>(*msg, senderIP);
discoveredDevices.push_back(device);
std::cout << "Discovered new Livox device: " << device->stringify()
<< std::endl;
}
void BroadcastListener::start(void)
{
if (isListening.load()) { return; }
try
{
/** EXPLANATION:
* Set up a boost::asio udp listening socket on the broadcast listening
* port.
*
* FIXME:
* We should also set up a timer to check for devices that have gone
* away.
*/
socket.open(boost::asio::ip::udp::v4());
socket.bind(listeningEndpoint);
isListening.store(true);
// Start the first async receive operation
startReceive();
std::cout << "BroadcastListener started on port " << listeningPort
<< std::endl;
}
catch (const boost::system::system_error& e)
{
isListening.store(false);
std::cerr << "Failed to start BroadcastListener: " << e.what()
<< std::endl;
throw;
}
}
void BroadcastListener::startReceive(void)
{
if (!isListening.load()) { return; }
socket.async_receive_from(
boost::asio::buffer(bcastMsgRecvBuffer, sizeof(bcastMsgRecvBuffer)),
senderEndpoint,
[this](const boost::system::error_code& ec, std::size_t bytes_received)
{
broadcastMsgInd(ec, bytes_received);
// Continue listening for the next packet
if (isListening.load())
{ startReceive(); }
}
);
}
void BroadcastListener::stop(void)
{
if (!isListening.load()) { return; }
isListening.store(false);
try
{
socket.close();
std::cout << "BroadcastListener stopped" << std::endl;
}
catch (const boost::system::system_error& e)
{
std::cerr << "Error stopping BroadcastListener: " << e.what()
<< std::endl;
throw;
}
}
} // namespace comms
} // namespace livoxProto1
@@ -0,0 +1,175 @@
#ifndef LIVOXPROTO1_PROTOCOL_H
#define LIVOXPROTO1_PROTOCOL_H
#include <vector>
#include <string>
#include <memory>
#include <sstream>
#include <atomic>
#include <user/senseApiDesc.h>
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));
/** 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;
};
/** EXPLANATION:
* This class merely listens for UDP bcast dgrams on the designated listening
* port. It then builds a list of client device IP addrs that it has heard from.
* It doesn't connect to them or signal any events to the rest of the lib,
* except in the case that a device which the lib is using has gone away.
*
* Other than that, its role is to tell the lib which devices are available
* on the network.
*/
#define UDP_BCAST_MSG_BUFFER_NBYTES (1024)
class BroadcastListener
{
public:
BroadcastListener(
const std::shared_ptr<smo::ComponentThread>& componentThread,
uint16_t listeningPort=55000, uint16_t connectPort=65000);
~BroadcastListener() = default;
typedef void (DeviceGoneAwayCbFn)(const DiscoveredDevice &device);
void setDeviceGoneAwayCb(DeviceGoneAwayCbFn *cb)
{ deviceGoneAwayCb = cb; }
bool deviceExists(const std::string &deviceIdentifier) const
{ return getDevice(deviceIdentifier) != nullptr; }
std::shared_ptr<DiscoveredDevice>
getDevice(const std::string &deviceIdentifier) const;
void start(void);
void stop(void);
void broadcastMsgInd(
const boost::system::error_code& ec, std::size_t bytes_received);
private:
void startReceive(void);
private:
std::shared_ptr<smo::ComponentThread> componentThread;
/** EXPLANATION:
* The Livox proto says that client devices will spam broadcast UDP
* dgrams to us on the listening port. We can then use the source IP from
* the bcast dgram to figure out the client device's IP addr. Then we
* should send a connect dgram to the connect port. This will tell the
* client device our IP addr.
*/
uint16_t listeningPort, connectPort;
DeviceGoneAwayCbFn *deviceGoneAwayCb;
std::vector<std::shared_ptr<DiscoveredDevice>> discoveredDevices;
boost::asio::ip::udp::socket socket;
boost::asio::ip::udp::endpoint listeningEndpoint, senderEndpoint;
std::atomic<bool> isListening;
uint8_t bcastMsgRecvBuffer[UDP_BCAST_MSG_BUFFER_NBYTES];
};
} // namespace comms
} // namespace livoxProto1
#endif // LIVOXPROTO1_PROTOCOL_H
+1
View File
@@ -1 +1,2 @@
add_subdirectory(xcbWindow)
add_subdirectory(livoxGen1)
+22
View File
@@ -0,0 +1,22 @@
cmake_dependent_option(ENABLE_SENSEAPI_livoxGen1
"Enable Livox Gen1 LiDAR sense API" OFF
"ENABLE_LIB_livoxProto1" ON)
if(ENABLE_SENSEAPI_livoxGen1)
add_library(livoxGen1 SHARED
livoxGen1.cpp
)
# Set config define for header generation
add_compile_definitions(CONFIG_SENSEAPI_LIVOXGEN1_ENABLED)
target_include_directories(livoxGen1 PUBLIC
${Boost_INCLUDE_DIRS}
${CMAKE_SOURCE_DIR}/commonLibs
)
target_link_libraries(livoxGen1
${Boost_LIBRARIES}
)
# Install rules
install(TARGETS livoxGen1 DESTINATION lib)
endif()
+159
View File
@@ -0,0 +1,159 @@
#include <iostream>
#include <memory>
#include <vector>
#include <string>
#include <map>
#include <functional>
#include <dlfcn.h>
#include <user/senseApiDesc.h>
#include <user/deviceAttachmentSpec.h>
#include <livoxProto1/livoxProto1.h>
namespace smo {
namespace sense_api {
// Salmanoff hooks, obtained from SMO_GET_SENSE_API_DESC_FN_NAME().
static const SmoCallbacks* smoHooksPtr = nullptr;
static SmoThreadingModelDesc smoThreadingModelDesc;
// LivoxProto1 library state
struct LivoxProto1DllState
{
LivoxProto1DllState()
: dlopenHandle(nullptr, DlCloser),
livoxProto1_main(nullptr),
livoxProto1_exit(nullptr)
{}
static void DlCloser(void* handle)
{
if (handle) {
dlclose(handle);
}
}
std::unique_ptr<void, void(*)(void*)> dlopenHandle;
livoxProto1_mainFn *livoxProto1_main;
livoxProto1_exitFn *livoxProto1_exit;
};
static LivoxProto1DllState livoxProto1;
// Callback function declarations
extern "C" int livoxGen1_initializeInd(void);
extern "C" int livoxGen1_finalizeInd(void);
extern "C" int livoxGen1_attachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc);
extern "C" int livoxGen1_detachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc);
// Sense API descriptor
static const SenseApiDesc livoxGen1ApiDesc = {
.name = "livoxGen1",
.exportedImplexorApis = {
{.name = "pointCloudCoords"},
{.name = "pointCloudIntensity"},
{.name = "gyro"},
{.name = "accel"}
},
.sal_mgmt_libOps = {
.initializeInd = livoxGen1_initializeInd,
.finalizeInd = livoxGen1_finalizeInd,
.attachDeviceReq = livoxGen1_attachDeviceReq,
.detachDeviceReq = livoxGen1_detachDeviceReq
}
};
// Callback function implementations
extern "C" int livoxGen1_initializeInd(void)
{
if (!smoHooksPtr)
{
throw std::runtime_error(std::string(__func__) + ": SMO hooks "
"pointers not filled in.");
}
// Load LivoxProto1 library
auto libPath = smoHooksPtr->searchForLibInSmoSearchPaths(
"liblivoxProto1.so");
livoxProto1.dlopenHandle.reset(dlopen(
libPath.value_or("liblivoxProto1.so").c_str(), RTLD_LAZY));
if (!livoxProto1.dlopenHandle)
{
throw std::runtime_error(
std::string(__func__) +
": Failed to load LivoxProto1 library: " +
(dlerror() ? dlerror() : "unknown error"));
}
// Get LivoxProto1 library functions
livoxProto1.livoxProto1_main = reinterpret_cast<livoxProto1_mainFn *>(
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit) {
throw std::runtime_error(
std::string(__func__) +
": Failed to get LivoxProto1 library functions");
}
// Call LivoxProto1 library main function
livoxProto1.livoxProto1_main(smoThreadingModelDesc.componentThread);
return 0; // Success
}
extern "C" int livoxGen1_finalizeInd(void)
{
// TODO: Implement finalization logic
if (livoxProto1.livoxProto1_exit) {
livoxProto1.livoxProto1_exit();
}
if (livoxProto1.dlopenHandle)
{
dlclose(livoxProto1.dlopenHandle.get());
livoxProto1.dlopenHandle.reset();
}
livoxProto1 = LivoxProto1DllState();
return 0; // Success
}
extern "C" int livoxGen1_attachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
)
{
// TODO: Implement device attachment logic
(void)desc; // Suppress unused parameter warning
return 0; // Success
}
extern "C" int livoxGen1_detachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
)
{
// TODO: Implement device detachment logic
(void)desc; // Suppress unused parameter warning
return 0; // Success
}
// Exported function
extern "C" smo::sense_api::SMO_GET_SENSE_API_DESC_FN_TYPEDEF
SMO_GET_SENSE_API_DESC_FN_NAME;
const smo::sense_api::SenseApiDesc& SMO_GET_SENSE_API_DESC_FN_NAME(
const smo::sense_api::SmoCallbacks& callbacks,
const smo::sense_api::SmoThreadingModelDesc& threadingModel)
{
smoHooksPtr = &callbacks;
smoThreadingModelDesc = threadingModel;
return livoxGen1ApiDesc;
}
} // namespace sense_api
} // namespace smo