diff --git a/stimBuffApis/livoxGen1/livoxGen1.cpp b/stimBuffApis/livoxGen1/livoxGen1.cpp index 7f6e7eb..13f8f76 100644 --- a/stimBuffApis/livoxGen1/livoxGen1.cpp +++ b/stimBuffApis/livoxGen1/livoxGen1.cpp @@ -55,7 +55,8 @@ struct LivoxProto1DllState livoxProto1_getOrCreateDeviceReq(nullptr), livoxProto1_destroyDeviceReq(nullptr), livoxProto1_device_enablePcloudDataReq(nullptr), - livoxProto1_device_disablePcloudDataReq(nullptr) + livoxProto1_device_disablePcloudDataReq(nullptr), + livoxProto1_device_getReturnModeReq(nullptr) {} static void DlCloser(void* handle) @@ -74,6 +75,7 @@ struct LivoxProto1DllState *livoxProto1_device_enablePcloudDataReq; livoxProto1_device_disablePcloudDataReqFn *livoxProto1_device_disablePcloudDataReq; + livoxProto1_device_getReturnModeReqFn *livoxProto1_device_getReturnModeReq; }; static LivoxProto1DllState livoxProto1; @@ -131,28 +133,65 @@ public: /* Delay here because getOrCreate just sent HandshakeReq, so device * may not yet be ready for another command. */ - context->delayedEnablePcloudData(context); + context->delayedGetReturnMode(context); + } + + void delayedGetReturnMode( + std::shared_ptr context) + { + // Initialize timer with device's component thread + delayTimer = std::make_unique( + context->stimBuff->device->componentThread->getIoService()); + + delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); + delayTimer->async_wait( + std::bind( + &AttachDeviceReq::attachDeviceReq2, + context.get(), context, + std::placeholders::_1)); } void attachDeviceReq2( std::shared_ptr context, - bool success) + const boost::system::error_code& error) + { + if (error) + { + std::cerr << __func__ << ": Timer error: " << error.message() + << std::endl; + context->callOriginalCb(false, context->spec); + return; + } + + (*livoxProto1.livoxProto1_device_getReturnModeReq)( + context->stimBuff->device, + {context, std::bind( + &AttachDeviceReq::attachDeviceReq3, + context.get(), context, + std::placeholders::_1, std::placeholders::_2)}); + } + + void attachDeviceReq3( + std::shared_ptr context, + bool success, uint8_t mode) { if (!success) { - std::cerr << __func__ << ": Failed to enable pcloud data for dev " + std::cerr << __func__ << ": Failed to get return mode for dev " << context->spec->deviceSelector << std::endl; - // Fallthrough. + context->callOriginalCb(false, context->spec); + return; } if (1 || OptionParser::getOptions().verbose) { - std::cout << __func__ << ": Enabled pcloud data for device: " - << context->spec->deviceSelector << std::endl; + std::cout << __func__ << ": Got return mode (" << (int)mode + << ") for device: " << context->spec->deviceSelector + << std::endl; } - context->callOriginalCb(success, context->spec); + context->delayedEnablePcloudData(context); } // Helper method to delay and then call enablePcloudDataReq @@ -166,13 +205,12 @@ public: delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); delayTimer->async_wait( std::bind( - &AttachDeviceReq::delayedEnablePcloudDataCallback, + &AttachDeviceReq::attachDeviceReq4, context.get(), context, std::placeholders::_1)); } - // Callback for the delayed enablePcloudDataReq - void delayedEnablePcloudDataCallback( + void attachDeviceReq4( std::shared_ptr context, const boost::system::error_code& error) { @@ -187,10 +225,32 @@ public: (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( context->stimBuff->device, {context, std::bind( - &AttachDeviceReq::attachDeviceReq2, + &AttachDeviceReq::attachDeviceReq5, context.get(), context, std::placeholders::_1)}); } + + void attachDeviceReq5( + std::shared_ptr context, + bool success) + { + if (!success) + { + std::cerr << __func__ << ": Failed to enable pcloud data for dev " + << context->spec->deviceSelector << std::endl; + + context->callOriginalCb(false, context->spec); + return; + } + + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Enabled pcloud data for device: " + << context->spec->deviceSelector << std::endl; + } + + context->callOriginalCb(success, context->spec); + } }; class DetachDeviceReq @@ -373,12 +433,18 @@ extern "C" int livoxGen1_initializeInd(void) dlsym( livoxProto1.dlopenHandle.get(), "livoxProto1_device_disablePcloudDataReq")); + livoxProto1.livoxProto1_device_getReturnModeReq = reinterpret_cast< + livoxProto1_device_getReturnModeReqFn *>( + dlsym( + livoxProto1.dlopenHandle.get(), + "livoxProto1_device_getReturnModeReq")); if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit || !livoxProto1.livoxProto1_getOrCreateDeviceReq || !livoxProto1.livoxProto1_destroyDeviceReq || !livoxProto1.livoxProto1_device_enablePcloudDataReq - || !livoxProto1.livoxProto1_device_disablePcloudDataReq) + || !livoxProto1.livoxProto1_device_disablePcloudDataReq + || !livoxProto1.livoxProto1_device_getReturnModeReq) { throw std::runtime_error( std::string(__func__) + @@ -443,16 +509,15 @@ extern "C" void livoxGen1_attachDeviceReq( return; } - /* Enable pcloud data. Don't need delay since this no commands were + /* Get return mode first. Don't need delay since this no commands were * sent to device prior to us reaching here. */ - (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( + (*livoxProto1.livoxProto1_device_getReturnModeReq)( pcloudStimBuff->device, {request, std::bind( - &AttachDeviceReq::attachDeviceReq2, + &AttachDeviceReq::attachDeviceReq3, request.get(), request, - std::placeholders::_1)}); - + std::placeholders::_1, std::placeholders::_2)}); return; }