LivoxGen1: Add get/setReturnModeReq()

commonLibs/livoxProto1/device.cpp

@@ -193,7 +193,7 @@ public:
 		// Fail early - if this also failed, all connection attempts failed
 		if (!success)
 		{
-			context->callOriginalCb(false);
+			context->callOriginalCallbackWithFailure();
 			return;
 		}
 
@@ -213,7 +213,7 @@ public:
 		if (error)
 		{
 			// Timer was cancelled or error occurred
-			context->callOriginalCb(false);
+			context->callOriginalCallbackWithFailure();
 			return;
 		}
 
@@ -221,7 +221,7 @@ public:
 			{context, std::bind(&ConnectReq::connectReq4,
 			context.get(), context,
 			std::placeholders::_1)});
-	}
+		}
 
 	void connectReq4(
 		std::shared_ptr<ConnectReq> context,
@@ -234,12 +234,20 @@ public:
 				"device (" << context->device.discoveredDevice.deviceIdentifier
 				<< ") @(" << context->device.discoveredDevice.ipAddr << ").\n";
 
-			context->callOriginalCb(false);
+			context->callOriginalCallbackWithFailure();
 			return;
 		}
 
-		context->callOriginalCb(success);
+		context->callOriginalCallback(success);
 	}
+
+	// Public accessor for the original callback
+	void callOriginalCallback(bool success)
+		{ callOriginalCb(success); }
+
+	// Wrapper for failure cases
+	void callOriginalCallbackWithFailure()
+		{ callOriginalCallback(false); }
 };
 
 void Device::connectReq(smo::Callback<Device::connectReqCbFn> callback)
@@ -1957,4 +1965,550 @@ void Device::unregisterUdpCommandHandler(
 	}
 }
 
+// SetReturnModeReq continuation class
+class Device::SetReturnModeReq
+:	public smo::NonPostedAsynchronousContinuation<Device::setReturnModeReqCbFn>
+{
+public:
+	enum class SocketState
+	{
+		SOCKET_STILL_WAITING = 0,
+		SOCKET_ERROR,
+		SOCKET_RECV_SUCCESS,
+		SOCKET_RECV_ERROR
+	};
+
+public:
+	Device& device;
+	uint8_t returnMode;
+
+	// Atomic state flags for async coordination
+	std::atomic<bool> timerFired{false};
+	std::atomic<SocketState> socketState{SocketState::SOCKET_STILL_WAITING};
+	std::atomic<bool> handlerExecuted{false};
+
+	// The timeout timer.
+	boost::asio::deadline_timer timeoutTimer;
+
+	// Received data storage
+	uint8_t responseBuffer[1024]{};
+	ssize_t bytesReceived = -1;
+	struct sockaddr_in senderAddr;
+	socklen_t senderAddrLen = sizeof(senderAddr);
+
+public:
+	friend void Device::setReturnModeReq(
+		uint8_t returnMode,
+		smo::Callback<Device::setReturnModeReqCbFn> callback);
+
+	SetReturnModeReq(
+		Device& dev, uint8_t mode,
+		smo::Callback<Device::setReturnModeReqCbFn> cb)
+	:	smo::NonPostedAsynchronousContinuation<Device::setReturnModeReqCbFn>(
+			std::move(cb)),
+	device(dev), returnMode(mode),
+	timeoutTimer(device.componentThread->getIoService())
+	{}
+
+	virtual ~SetReturnModeReq()
+	{
+		cleanup();
+	}
+
+	// Public accessor for the original callback
+	void callOriginalCallback(bool success)
+		{ this->callOriginalCb(success); }
+
+	void callOriginalCallbackWithFailure()
+		{ this->callOriginalCb(false); }
+
+	void setupAsyncCallbacks(std::shared_ptr<SetReturnModeReq> request)
+	{
+		// Set up timeout timer
+		timeoutTimer.expires_from_now(boost::posix_time::milliseconds(
+			device.handshakeTimeoutMs));
+		timeoutTimer.async_wait(
+			std::bind(&SetReturnModeReq::setReturnModeReq1_1,
+				this, request,
+				std::placeholders::_1));
+
+		// Register UDP command handler for set return mode response (cmd_set=0x01, cmd_id=0x06)
+		device.registerUdpCommandHandler(
+			0x01, 0x06,
+			std::bind(&SetReturnModeReq::setReturnModeReq1_2,
+				this, request,
+				std::placeholders::_1, std::placeholders::_2,
+				std::placeholders::_3),
+			device.discoveredDevice.ipAddr);
+	}
+
+	void setReturnModeReq1_1(
+		std::shared_ptr<SetReturnModeReq> context,
+		const boost::system::error_code& error
+	)
+	{
+		if (error == boost::asio::error::operation_aborted) {
+			// Timer was cancelled, ignore
+			return;
+		}
+
+		context->timerFired.store(true);
+		context->setReturnModeReq2(context);
+	}
+
+	void setReturnModeReq1_2(
+		std::shared_ptr<SetReturnModeReq> context,
+		const uint8_t* data, ssize_t bytesReceived,
+		const struct sockaddr_in& senderAddr
+	)
+	{
+		// Store the received data
+		context->bytesReceived = bytesReceived;
+		context->senderAddr = senderAddr;
+		context->senderAddrLen = sizeof(senderAddr);
+
+		// Copy the data to our buffer
+		if (bytesReceived > 0
+			&& bytesReceived <= (ssize_t)sizeof(context->responseBuffer))
+		{
+			memcpy(context->responseBuffer, data, bytesReceived);
+			context->socketState = SocketState::SOCKET_RECV_SUCCESS;
+		} else
+		{
+			context->socketState = SocketState::SOCKET_RECV_ERROR;
+			std::cerr << __func__ << ": Invalid data size: " << bytesReceived
+				<< std::endl;
+		}
+
+		context->setReturnModeReq2(context);
+	}
+
+	void setReturnModeReq2(
+		std::shared_ptr<SetReturnModeReq> context
+	)
+	{
+		// Only execute once
+		if (context->handlerExecuted.exchange(true)) { return; }
+
+		context->timeoutTimer.cancel();
+		device.unregisterUdpCommandHandler(
+			0x01, 0x06, device.discoveredDevice.ipAddr);
+
+		SocketState finalSocketState = context->socketState.load();
+		bool finalTimerFired = context->timerFired.load();
+
+		// Check for timeout only if there was no socket activity
+		if (finalTimerFired
+			&& finalSocketState == SocketState::SOCKET_STILL_WAITING)
+		{
+			std::cerr << __func__ << ": Set return mode timeout with "
+				<< device.discoveredDevice.ipAddr << "("
+				<< device.discoveredDevice.deviceIdentifier << ")" << "\n";
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		// Socket error from boost::asio
+		if (finalSocketState == SocketState::SOCKET_ERROR)
+		{
+			std::cerr << __func__ << ": Socket error during set return mode with "
+				<< device.discoveredDevice.ipAddr << std::endl;
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		if (finalSocketState == SocketState::SOCKET_RECV_ERROR)
+		{
+			std::cerr << __func__ << ": Receive error during set return mode with "
+				<< device.discoveredDevice.ipAddr << "\n";
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		/* Result must have been RECV_SUCCESS state if we reach here.
+		 * Data was already read in the async callback, just validate it
+		 */
+		if (context->bytesReceived
+			< static_cast<ssize_t>(sizeof(comms::SetLiDARReturnModeResponse)))
+		{
+			std::cerr << __func__ << ": Response of size "
+				<< context->bytesReceived << " too small from "
+				<< device.discoveredDevice.ipAddr << "\n";
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		comms::SetLiDARReturnModeResponse* response =
+			reinterpret_cast<comms::SetLiDARReturnModeResponse*>(
+				context->responseBuffer);
+		response->swapContentsToHostEndianness();
+
+		// Early callback return on error; success path only if all checks pass
+		if (response->command.cmd_set != 0x01 ||
+			response->command.cmd_id != 0x06 ||
+			response->ret_code != 0x00)
+		{
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		context->callOriginalCallback(true);
+	}
+
+	void cleanup()
+	{
+		timeoutTimer.cancel();
+	}
+
+	bool sendCommand()
+	{
+		// Get the command endpoint from the UdpCommandDemuxer
+		auto& protoState = livoxProto1::getProtoState();
+		if (!protoState.deviceManager)
+		{
+			std::cerr << __func__ << ": No device manager available.\n";
+			return false;
+		}
+
+		auto cmdEndpointFdDesc = protoState.deviceManager->udpCommandDemuxer
+			.getCmdEndpointFdDesc();
+		if (!cmdEndpointFdDesc)
+		{
+			std::cerr << __func__ << ": No command endpoint available.\n";
+			return false;
+		}
+
+		// Create set return mode message
+		comms::SetLiDARReturnMode setReturnModeMsg;
+		setReturnModeMsg.mode = returnMode;
+		setReturnModeMsg.swapContentsToProtocolEndianness();
+		setReturnModeMsg.header.setCrc16FromRawBytes();
+		setReturnModeMsg.header.swapCrc16ToProtocolEndianness();
+		setReturnModeMsg.footer.crc_32 = setReturnModeMsg.calculateCrc32();
+		setReturnModeMsg.footer.swapCrc32ToProtocolEndianness();
+
+		// Set up destination address
+		struct sockaddr_in deviceAddr;
+		deviceAddr.sin_family = AF_INET;
+		deviceAddr.sin_addr.s_addr = inet_addr(
+			device.discoveredDevice.ipAddr.c_str());
+		deviceAddr.sin_port = htons(65000); // Commands go to port 65000
+
+		// Send set return mode message
+		ssize_t bytesSent = sendto(
+			cmdEndpointFdDesc->native_handle(),
+			&setReturnModeMsg, sizeof(setReturnModeMsg), 0,
+			(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));
+
+		if (bytesSent < 0)
+		{
+			std::cerr << __func__ << ": Failed to send set return mode message: "
+				<< strerror(errno) << std::endl;
+			return false;
+		}
+
+		std::cout << __func__ << ": Sent set return mode message to "
+			<< device.discoveredDevice.ipAddr << ":" << 65000 << std::endl;
+		return true;
+	}
+};
+
+// GetReturnModeReq continuation class
+class Device::GetReturnModeReq
+:	public smo::NonPostedAsynchronousContinuation<Device::getReturnModeReqCbFn>
+{
+public:
+	enum class SocketState
+	{
+		SOCKET_STILL_WAITING = 0,
+		SOCKET_ERROR,
+		SOCKET_RECV_SUCCESS,
+		SOCKET_RECV_ERROR
+	};
+
+public:
+	Device& device;
+
+	// Atomic state flags for async coordination
+	std::atomic<bool> timerFired{false};
+	std::atomic<SocketState> socketState{SocketState::SOCKET_STILL_WAITING};
+	std::atomic<bool> handlerExecuted{false};
+
+	// The timeout timer.
+	boost::asio::deadline_timer timeoutTimer;
+
+	// Received data storage
+	uint8_t responseBuffer[1024]{};
+	ssize_t bytesReceived = -1;
+	struct sockaddr_in senderAddr;
+	socklen_t senderAddrLen = sizeof(senderAddr);
+
+public:
+	friend void Device::getReturnModeReq(
+		smo::Callback<Device::getReturnModeReqCbFn> callback);
+
+	GetReturnModeReq(
+		Device& dev,
+		smo::Callback<Device::getReturnModeReqCbFn> cb)
+	:	smo::NonPostedAsynchronousContinuation<Device::getReturnModeReqCbFn>(
+			std::move(cb)),
+	device(dev),
+	timeoutTimer(device.componentThread->getIoService())
+	{}
+
+	virtual ~GetReturnModeReq()
+	{
+		cleanup();
+	}
+
+	// Public accessor for the original callback
+	void callOriginalCallback(bool success, uint8_t returnMode)
+		{ this->callOriginalCb(success, returnMode); }
+
+	void callOriginalCallbackWithFailure()
+		{ this->callOriginalCb(false, 0); }
+
+	void setupAsyncCallbacks(std::shared_ptr<GetReturnModeReq> request)
+	{
+		// Set up timeout timer
+		timeoutTimer.expires_from_now(boost::posix_time::milliseconds(
+			device.handshakeTimeoutMs));
+
+		timeoutTimer.async_wait(
+			std::bind(&GetReturnModeReq::getReturnModeReq1_1,
+				this, request,
+				std::placeholders::_1));
+
+		// Register UDP command handler for get return mode response (cmd_set=0x01, cmd_id=0x07)
+		device.registerUdpCommandHandler(
+			0x01, 0x07,
+			std::bind(&GetReturnModeReq::getReturnModeReq1_2,
+				this, request,
+				std::placeholders::_1, std::placeholders::_2,
+				std::placeholders::_3),
+			device.discoveredDevice.ipAddr);
+	}
+
+	void getReturnModeReq1_1(
+		std::shared_ptr<GetReturnModeReq> context,
+		const boost::system::error_code& error
+	)
+	{
+		if (error == boost::asio::error::operation_aborted) {
+			// Timer was cancelled, ignore
+			return;
+		}
+
+		context->timerFired.store(true);
+		context->getReturnModeReq2(context);
+	}
+
+	void getReturnModeReq1_2(
+		std::shared_ptr<GetReturnModeReq> context,
+		const uint8_t* data, ssize_t bytesReceived,
+		const struct sockaddr_in& senderAddr
+	)
+	{
+		// Store the received data
+		context->bytesReceived = bytesReceived;
+		context->senderAddr = senderAddr;
+		context->senderAddrLen = sizeof(senderAddr);
+
+		// Copy the data to our buffer
+		if (bytesReceived > 0
+			&& bytesReceived <= (ssize_t)sizeof(context->responseBuffer))
+		{
+			memcpy(context->responseBuffer, data, bytesReceived);
+			context->socketState = SocketState::SOCKET_RECV_SUCCESS;
+		} else
+		{
+			context->socketState = SocketState::SOCKET_RECV_ERROR;
+			std::cerr << __func__ << ": Invalid data size: " << bytesReceived
+				<< std::endl;
+		}
+
+		context->getReturnModeReq2(context);
+	}
+
+	void getReturnModeReq2(
+		std::shared_ptr<GetReturnModeReq> context
+	)
+	{
+		// Only execute once
+		if (context->handlerExecuted.exchange(true)) { return; }
+
+		context->timeoutTimer.cancel();
+		device.unregisterUdpCommandHandler(
+			0x01, 0x07, device.discoveredDevice.ipAddr);
+
+		SocketState finalSocketState = context->socketState.load();
+		bool finalTimerFired = context->timerFired.load();
+
+		// Check for timeout only if there was no socket activity
+		if (finalTimerFired
+			&& finalSocketState == SocketState::SOCKET_STILL_WAITING)
+		{
+			std::cerr << __func__ << ": Get return mode timeout with "
+				<< device.discoveredDevice.ipAddr << "("
+				<< device.discoveredDevice.deviceIdentifier
+				<< ")" << "\n";
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+		// Socket error from boost::asio
+		if (finalSocketState == SocketState::SOCKET_ERROR)
+		{
+			std::cerr << __func__ << ": Socket error during get return mode with "
+				<< device.discoveredDevice.ipAddr << std::endl;
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+		if (finalSocketState == SocketState::SOCKET_RECV_ERROR)
+		{
+			std::cerr << __func__ << ": Receive error during get return mode with "
+				<< device.discoveredDevice.ipAddr << std::endl;
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+		/* Result must have been RECV_SUCCESS state if we reach here.
+		 * Data was already read in the async callback, just validate it
+		 */
+		if (context->bytesReceived
+			< static_cast<ssize_t>(sizeof(comms::GetLiDARReturnModeResponse)))
+		{
+			std::cerr << __func__ << ": Response of size "
+				<< context->bytesReceived << " too small from "
+				<< device.discoveredDevice.ipAddr << std::endl;
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		comms::GetLiDARReturnModeResponse* response =
+			reinterpret_cast<comms::GetLiDARReturnModeResponse*>(
+				context->responseBuffer);
+		response->swapContentsToHostEndianness();
+
+		// Check if response indicates success
+		if (!(response->command.cmd_set == 0x01 &&
+		      response->command.cmd_id == 0x07 &&
+		      response->ret_code == 0x00))
+		{
+			context->callOriginalCallbackWithFailure();
+			return;
+		}
+
+		context->callOriginalCallback(true, response->mode);
+	}
+
+	void cleanup()
+	{
+		timeoutTimer.cancel();
+	}
+
+	bool sendCommand()
+	{
+		// Get the command endpoint from the UdpCommandDemuxer
+		auto& protoState = livoxProto1::getProtoState();
+		if (!protoState.deviceManager)
+		{
+			std::cerr << __func__ << ": No device manager available.\n";
+			return false;
+		}
+
+		auto cmdEndpointFdDesc = protoState.deviceManager->udpCommandDemuxer
+			.getCmdEndpointFdDesc();
+		if (!cmdEndpointFdDesc)
+		{
+			std::cerr << __func__ << ": No command endpoint available.\n";
+			return false;
+		}
+
+		// Create get return mode message
+		comms::GetLiDARReturnMode getReturnModeMsg;
+		getReturnModeMsg.swapContentsToProtocolEndianness();
+		getReturnModeMsg.header.setCrc16FromRawBytes();
+		getReturnModeMsg.header.swapCrc16ToProtocolEndianness();
+		getReturnModeMsg.footer.crc_32 = getReturnModeMsg.calculateCrc32();
+		getReturnModeMsg.footer.swapCrc32ToProtocolEndianness();
+
+		// Set up destination address
+		struct sockaddr_in deviceAddr;
+		deviceAddr.sin_family = AF_INET;
+		deviceAddr.sin_addr.s_addr = inet_addr(device.discoveredDevice.ipAddr.c_str());
+		deviceAddr.sin_port = htons(65000); // Commands go to port 65000
+
+		// Send get return mode message
+		ssize_t bytesSent = sendto(
+			cmdEndpointFdDesc->native_handle(),
+			&getReturnModeMsg, sizeof(getReturnModeMsg), 0,
+			(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));
+
+		if (bytesSent < 0)
+		{
+			std::cerr << __func__ << ": Failed to send get return mode message: "
+				<< strerror(errno) << std::endl;
+			return false;
+		}
+
+		std::cout << __func__ << ": Sent get return mode message to "
+			<< device.discoveredDevice.ipAddr << ":" << 65000 << std::endl;
+		return true;
+	}
+};
+
+void Device::setReturnModeReq(
+	uint8_t returnMode,
+	smo::Callback<Device::setReturnModeReqCbFn> callback
+	)
+{
+	auto request = std::make_shared<SetReturnModeReq>(
+		*this, returnMode, std::move(callback));
+
+	// Check if device IP is available
+	if (discoveredDevice.ipAddr.empty())
+	{
+		std::cerr << __func__ << ": No device IP available for device "
+			<< discoveredDevice.deviceIdentifier << std::endl;
+		request->callOriginalCallbackWithFailure();
+		return;
+	}
+
+	// Send the set return mode command
+	if (!request->sendCommand())
+	{
+		request->callOriginalCallbackWithFailure();
+		return;
+	}
+
+	// Setup async callbacks
+	request->setupAsyncCallbacks(request);
+}
+
+void Device::getReturnModeReq(
+	smo::Callback<Device::getReturnModeReqCbFn> callback
+	)
+{
+	auto request = std::make_shared<GetReturnModeReq>(
+		*this, std::move(callback));
+
+	// Check if device IP is available
+	if (discoveredDevice.ipAddr.empty())
+	{
+		std::cerr << __func__ << ": No device IP available for device "
+			<< discoveredDevice.deviceIdentifier << std::endl;
+		request->callOriginalCallbackWithFailure();
+		return;
+	}
+
+	// Send the get return mode command
+	if (!request->sendCommand())
+	{
+		request->callOriginalCallbackWithFailure();
+		return;
+	}
+
+	// Setup async callbacks
+	request->setupAsyncCallbacks(request);
+}
+
 } // namespace livoxProto1

commonLibs/livoxProto1/device.h

@@ -110,6 +110,8 @@ private:
 	class DisconnectReq;
 	class EnablePcloudDataReq;
 	class DisablePcloudDataReq;
+	class SetReturnModeReq;
+	class GetReturnModeReq;
 
 public:
 	// Utility methods
@@ -127,6 +129,9 @@ public:
 	typedef std::function<void(bool success)> disconnectReqCbFn;
 	typedef std::function<void(bool success)> enablePcloudDataReqCbFn;
 	typedef std::function<void(bool success)> disablePcloudDataReqCbFn;
+	typedef std::function<void(bool success)> setReturnModeReqCbFn;
+	typedef std::function<void(bool success, uint8_t returnMode)>
+		getReturnModeReqCbFn;
 
 	// Async connection methods
 	void connectReq(smo::Callback<connectReqCbFn> callback);
@@ -140,6 +145,9 @@ public:
 	void disconnectReq(smo::Callback<disconnectReqCbFn> callback);
 	void enablePcloudDataReq(smo::Callback<enablePcloudDataReqCbFn> callback);
 	void disablePcloudDataReq(smo::Callback<disablePcloudDataReqCbFn> callback);
+	void setReturnModeReq(
+		uint8_t returnMode, smo::Callback<setReturnModeReqCbFn> callback);
+	void getReturnModeReq(smo::Callback<getReturnModeReqCbFn> callback);
 
 	// Heartbeat state
 	std::unique_ptr<boost::asio::deadline_timer> heartbeatTimer;