From 73b2d981f975c8316428d0d311e286e862bbf937 Mon Sep 17 00:00:00 2001 From: Hayodea Hakol Date: Tue, 9 Sep 2025 12:09:59 -0400 Subject: [PATCH] livoxGen1: Implement attach/detachDeviceReq by sync-bridging Proto1 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. --- senseApis/livoxGen1/livoxGen1.cpp | 106 +++++++++++++++++++++++++----- 1 file changed, 90 insertions(+), 16 deletions(-) diff --git a/senseApis/livoxGen1/livoxGen1.cpp b/senseApis/livoxGen1/livoxGen1.cpp index 7e4ae09..6410ea9 100644 --- a/senseApis/livoxGen1/livoxGen1.cpp +++ b/senseApis/livoxGen1/livoxGen1.cpp @@ -6,11 +6,13 @@ #include #include #include +#include #include #include #include #include + 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 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( 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& 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 callbackCalled{false}; + std::shared_ptr device = nullptr; + std::shared_ptr 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 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); - std::cout << __func__ << ": Successfully attached Livox device: " - << desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n"; + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Successfully attached Livox device: " + << 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 callbackCalled{false}; + bool retVal = false; + std::shared_ptr 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