hayodea/salmanoff
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 2 Wiki Activity

livoxGen1: Implement attach/detachDeviceReq by sync-bridging Proto1

Browse Source 
We perform bridged synchronous calls into liblivoxProto1 in order
to support attach/detachDeviceReq. We'll eventually make
attachDetachDeviceReq fully asynchronous but for now we're happy
that we have this working driver for this fairly tricky device.
This commit is contained in:
Hayodea Hekol
2025-09-09 12:09:59 -04:00
parent 20cdf64afb
commit 73b2d981f9
1 changed files with 90 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
senseApis/livoxGen1/livoxGen1.cpp
+89 -15
View File
@@ -6,11 +6,13 @@
#include <functional>
#include <algorithm>
#include <dlfcn.h>
#include <opts.h>
#include <user/senseApiDesc.h>
#include <user/deviceAttachmentSpec.h>
#include <livoxProto1/livoxProto1.h>
#include <livoxProto1/device.h>
namespace smo {
namespace sense_api {
@@ -25,7 +27,8 @@ struct LivoxProto1DllState
: dlopenHandle(nullptr, DlCloser),
livoxProto1_main(nullptr),
livoxProto1_exit(nullptr),
livoxProto1_getOrCreateDevice(nullptr)
livoxProto1_getOrCreateDeviceReq(nullptr),
livoxProto1_destroyDeviceReq(nullptr)
{}
static void DlCloser(void* handle)
@@ -38,7 +41,8 @@ struct LivoxProto1DllState
std::unique_ptr<void, void(*)(void*)> dlopenHandle;
livoxProto1_mainFn *livoxProto1_main;
livoxProto1_exitFn *livoxProto1_exit;
livoxProto1_getOrCreateDeviceFn *livoxProto1_getOrCreateDevice;
livoxProto1_getOrCreateDeviceReqFn *livoxProto1_getOrCreateDeviceReq;
livoxProto1_destroyDeviceReqFn *livoxProto1_destroyDeviceReq;
};
static LivoxProto1DllState livoxProto1;
@@ -100,14 +104,20 @@ extern "C" int livoxGen1_initializeInd(void)
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
livoxProto1.livoxProto1_getOrCreateDevice = reinterpret_cast<
livoxProto1_getOrCreateDeviceFn *>(
livoxProto1.livoxProto1_getOrCreateDeviceReq = reinterpret_cast<
livoxProto1_getOrCreateDeviceReqFn *>(
dlsym(
livoxProto1.dlopenHandle.get(),
"livoxProto1_getOrCreateDevice"));
"livoxProto1_getOrCreateDeviceReq"));
livoxProto1.livoxProto1_destroyDeviceReq = reinterpret_cast<
livoxProto1_destroyDeviceReqFn *>(
dlsym(
livoxProto1.dlopenHandle.get(),
"livoxProto1_destroyDeviceReq"));
if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
|| !livoxProto1.livoxProto1_getOrCreateDevice)
|| !livoxProto1.livoxProto1_getOrCreateDeviceReq
|| !livoxProto1.livoxProto1_destroyDeviceReq)
{
throw std::runtime_error(
std::string(__func__) +
@@ -115,7 +125,8 @@ extern "C" int livoxGen1_initializeInd(void)
}
// Call LivoxProto1 library main function
(*livoxProto1.livoxProto1_main)(smoThreadingModelDesc.componentThread);
(*livoxProto1.livoxProto1_main)(
smoThreadingModelDesc.componentThread, *smoHooksPtr);
return 0; // Success
}
@@ -144,7 +155,7 @@ extern "C" int livoxGen1_attachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
)
{
if (!livoxProto1.livoxProto1_getOrCreateDevice)
if (!livoxProto1.livoxProto1_getOrCreateDeviceReq)
{
throw std::runtime_error(
std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
@@ -152,8 +163,15 @@ extern "C" int livoxGen1_attachDeviceReq(
}
// Parse integer parameters from provider params with defaults
int handshakeTimeoutMs = 250; // Default: 50ms
int retryDelayMs = 2000; // Default: 500ms
/* The Livox Avia will generally respond to a handshake request within
* 50ms. So we set the handshake timeout to 300ms to be safe.
*/
int handshakeTimeoutMs = 300; // Default: 50ms
/* Based on testing on a Livox Avia, the device will generally resume
* sending broadcast advertisement dgrams after about 5 seconds at most.
* Generally, it will resume sending them within 1-2 seconds.
*/
int retryDelayMs = 5250; // Default: 500ms
uint8_t smoSubnetNbits = 24; // Default: /24 subnet
uint16_t dataPort = 56000; // Default data port
uint16_t cmdPort = 56001; // Default command port
@@ -214,13 +232,36 @@ extern "C" int livoxGen1_attachDeviceReq(
}
}
// Call getOrCreateDevice with parsed parameters
auto device = (*livoxProto1.livoxProto1_getOrCreateDevice)(
std::atomic<bool> callbackCalled{false};
std::shared_ptr<livoxProto1::Device> device = nullptr;
std::shared_ptr<ComponentThread> self = smoHooksPtr->
ComponentThread_getSelf();
(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
desc->deviceSelector, // deviceIdentifier (broadcast code)
smoThreadingModelDesc.componentThread,
handshakeTimeoutMs, retryDelayMs,
smoIp, smoSubnetNbits,
dataPort, cmdPort, imuPort);
dataPort, cmdPort, imuPort,
[&callbackCalled, &device, self](
bool success, std::shared_ptr<livoxProto1::Device> dev) -> void
{
callbackCalled.store(true);
device = ((success) ? dev : nullptr);
// Ensure that the bridging loop below will get awakened.
self->getIoService().post([]{});
}
);
/** EXPLANATION:
* Bridge the async call by dequeueing until callbackCalled is true.
*/
for (;;)
{
self->getIoService().run_one();
if (callbackCalled.load() || self->getIoService().stopped())
{ break; }
}
if (!device)
{
@@ -230,8 +271,12 @@ extern "C" int livoxGen1_attachDeviceReq(
}
g_attachedDevices.push_back(device);
if (1 || OptionParser::getOptions().verbose)
{
std::cout << __func__ << ": Successfully attached Livox device: "
<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n";
<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier
<< ")\n";
}
return 0; // Success
}
@@ -263,10 +308,39 @@ extern "C" int livoxGen1_detachDeviceReq(
return -1; // Device not found
}
std::atomic<bool> callbackCalled{false};
bool retVal = false;
std::shared_ptr<ComponentThread> self = smoHooksPtr->
ComponentThread_getSelf();
(*livoxProto1.livoxProto1_destroyDeviceReq)(
*it,
[&callbackCalled, &retVal, self](bool success)
{
callbackCalled.store(true);
retVal = success;
self->getIoService().post([]{});
}
);
for (;;)
{
self->getIoService().run_one();
if (callbackCalled.load() || self->getIoService().stopped())
{ break; }
}
if (!retVal)
{
std::cerr << __func__ << ": Failed to destroy Livox device: "
<< desc->deviceIdentifier << std::endl;
}
g_attachedDevices.erase(it);
std::cout << __func__ << ": Successfully detached Livox device: "
<< desc->deviceIdentifier << "\n";
return 0; // Success
return 0;
}
// Exported function