LivoxProto1: Add en/disablePcloudDataReq()
Untested, but this should enable us to enable and disable data from the device.
This commit is contained in:
@@ -96,7 +96,9 @@ handshakeTimeoutMs(handshakeTimeoutMs), retryDelayMs(retryDelayMs),
|
|||||||
smoIp(smoIp), detectedSmoListeningIp(""), smoSubnetNbits(smoSubnetNbits),
|
smoIp(smoIp), detectedSmoListeningIp(""), smoSubnetNbits(smoSubnetNbits),
|
||||||
dataPort(dataPort), cmdPort(cmdPort), imuPort(imuPort),
|
dataPort(dataPort), cmdPort(cmdPort), imuPort(imuPort),
|
||||||
heartbeatFd(-1),
|
heartbeatFd(-1),
|
||||||
heartbeatActive(false)
|
heartbeatActive(false),
|
||||||
|
pcloudDataActive(false),
|
||||||
|
pcloudDataFd(-1)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -109,11 +111,23 @@ Device::~Device()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (pcloudDataActive.load()) {
|
||||||
|
pcloudDataActive.store(false);
|
||||||
|
if (pcloudDataSocketDesc) {
|
||||||
|
pcloudDataSocketDesc->cancel();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
heartbeatTimer.reset();
|
heartbeatTimer.reset();
|
||||||
|
pcloudDataSocketDesc.reset();
|
||||||
if (heartbeatFd >= 0) {
|
if (heartbeatFd >= 0) {
|
||||||
close(heartbeatFd);
|
close(heartbeatFd);
|
||||||
heartbeatFd = -1;
|
heartbeatFd = -1;
|
||||||
}
|
}
|
||||||
|
if (pcloudDataFd >= 0) {
|
||||||
|
close(pcloudDataFd);
|
||||||
|
pcloudDataFd = -1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -1207,4 +1221,511 @@ std::optional<std::string> Device::getSmoIp(const std::string& deviceIP)
|
|||||||
return std::nullopt;
|
return std::nullopt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Base class for both enable and disable pcloud data requests
|
||||||
|
template<typename CallbackType>
|
||||||
|
class EnDisablePcloudDataReq
|
||||||
|
: public smo::NonPostedAsynchronousContinuation<CallbackType>
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
/* This wrapper is just to enable us to use boost::stream_descriptor for its
|
||||||
|
* convenient API when waiting for the enable/disable ACK dgram.
|
||||||
|
*/
|
||||||
|
boost::asio::posix::stream_descriptor cmdResponseBoostFdWrapper;
|
||||||
|
|
||||||
|
// Received data storage
|
||||||
|
uint8_t responseBuffer[1024]{};
|
||||||
|
ssize_t bytesReceived = -1;
|
||||||
|
struct sockaddr_in senderAddr;
|
||||||
|
socklen_t senderAddrLen = sizeof(senderAddr);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
EnDisablePcloudDataReq(
|
||||||
|
Device& dev,
|
||||||
|
smo::Callback<CallbackType> cb)
|
||||||
|
: smo::NonPostedAsynchronousContinuation<CallbackType>(std::move(cb)),
|
||||||
|
device(dev),
|
||||||
|
timeoutTimer(device.componentThread->getIoService()),
|
||||||
|
cmdResponseBoostFdWrapper(device.componentThread->getIoService())
|
||||||
|
{}
|
||||||
|
|
||||||
|
public:
|
||||||
|
virtual ~EnDisablePcloudDataReq()
|
||||||
|
{
|
||||||
|
cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Public accessor for the original callback
|
||||||
|
void callOriginalCallback(bool success)
|
||||||
|
{ this->callOriginalCb(success); }
|
||||||
|
|
||||||
|
void callOriginalCallbackWithFailure()
|
||||||
|
{
|
||||||
|
/**
|
||||||
|
* EXPLANATION:
|
||||||
|
* We have to call cleanupCmdResponseFdBoostWrapper() here, specifically
|
||||||
|
* because there are self-references within this class that need to be
|
||||||
|
* cleaned up.
|
||||||
|
*
|
||||||
|
* The cmdResponseBoostFdWrapper holds a reference to the heartbeat
|
||||||
|
* socket for async operations. When the sequence fails, we need to
|
||||||
|
* break this reference to allow proper cleanup.
|
||||||
|
*
|
||||||
|
* Hence, we call cleanupCmdResponseFdBoostWrapper() at the point of
|
||||||
|
* failure.
|
||||||
|
*/
|
||||||
|
cleanupCmdResponseFdBoostWrapper();
|
||||||
|
callOriginalCallback(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cleanupCmdResponseFdBoostWrapper()
|
||||||
|
{
|
||||||
|
if (cmdResponseBoostFdWrapper.is_open()) {
|
||||||
|
cmdResponseBoostFdWrapper.release(); // Don't close heartbeat socket
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
bool setupSocket()
|
||||||
|
{
|
||||||
|
// Use the existing heartbeat socket for sending commands and receiving responses
|
||||||
|
if (device.heartbeatFd < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": No heartbeat socket available"
|
||||||
|
<< std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void setupAsyncCallbacks(
|
||||||
|
const std::shared_ptr<EnDisablePcloudDataReq<CallbackType>> &request
|
||||||
|
)
|
||||||
|
{
|
||||||
|
cmdResponseBoostFdWrapper.assign(device.heartbeatFd);
|
||||||
|
|
||||||
|
// Setup timeout timer
|
||||||
|
timeoutTimer.expires_from_now(
|
||||||
|
boost::posix_time::milliseconds(device.handshakeTimeoutMs));
|
||||||
|
|
||||||
|
timeoutTimer.async_wait(
|
||||||
|
std::bind(
|
||||||
|
&EnDisablePcloudDataReq<CallbackType>::enDisablePcloudDataReq1_1,
|
||||||
|
this, request,
|
||||||
|
std::placeholders::_1));
|
||||||
|
|
||||||
|
// Setup async wait for read-ready
|
||||||
|
cmdResponseBoostFdWrapper.async_wait(
|
||||||
|
boost::asio::posix::stream_descriptor::wait_read,
|
||||||
|
std::bind(
|
||||||
|
&EnDisablePcloudDataReq<CallbackType>::enDisablePcloudDataReq1_2,
|
||||||
|
this, request,
|
||||||
|
std::placeholders::_1));
|
||||||
|
}
|
||||||
|
|
||||||
|
void enDisablePcloudDataReq1_1(
|
||||||
|
std::shared_ptr<EnDisablePcloudDataReq<CallbackType>> context,
|
||||||
|
const boost::system::error_code& error
|
||||||
|
)
|
||||||
|
{
|
||||||
|
(void)error; // Suppress unused parameter warning
|
||||||
|
context->timerFired = true;
|
||||||
|
context->enDisablePcloudDataReq2(context);
|
||||||
|
}
|
||||||
|
|
||||||
|
void enDisablePcloudDataReq1_2(
|
||||||
|
std::shared_ptr<EnDisablePcloudDataReq<CallbackType>> context,
|
||||||
|
const boost::system::error_code& error
|
||||||
|
)
|
||||||
|
{
|
||||||
|
if (!error)
|
||||||
|
{
|
||||||
|
// Data is available for reading, perform the actual read
|
||||||
|
context->bytesReceived = recvfrom(
|
||||||
|
context->device.heartbeatFd,
|
||||||
|
context->responseBuffer, sizeof(context->responseBuffer), 0,
|
||||||
|
(struct sockaddr*)&context->senderAddr, &context->senderAddrLen);
|
||||||
|
|
||||||
|
if (context->bytesReceived > 0)
|
||||||
|
{ context->socketState = SocketState::SOCKET_RECV_SUCCESS; }
|
||||||
|
else
|
||||||
|
{ context->socketState = SocketState::SOCKET_RECV_ERROR; }
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{ context->socketState = SocketState::SOCKET_RECV_ERROR; }
|
||||||
|
|
||||||
|
context->enDisablePcloudDataReq2(context);
|
||||||
|
}
|
||||||
|
|
||||||
|
void enDisablePcloudDataReq2(
|
||||||
|
std::shared_ptr<EnDisablePcloudDataReq<CallbackType>> context
|
||||||
|
)
|
||||||
|
{
|
||||||
|
// Only execute once
|
||||||
|
if (context->handlerExecuted.exchange(true)) { return; }
|
||||||
|
|
||||||
|
SocketState finalSocketState = context->socketState.load();
|
||||||
|
bool finalTimerFired = context->timerFired.load();
|
||||||
|
|
||||||
|
context->timeoutTimer.cancel();
|
||||||
|
|
||||||
|
if (finalTimerFired &&
|
||||||
|
finalSocketState == SocketState::SOCKET_STILL_WAITING)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Command timeout for device "
|
||||||
|
<< context->device.discoveredDevice.deviceIdentifier
|
||||||
|
<< std::endl;
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (finalSocketState == SocketState::SOCKET_ERROR)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Socket error during command for device "
|
||||||
|
<< context->device.discoveredDevice.deviceIdentifier
|
||||||
|
<< std::endl;
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (finalSocketState == SocketState::SOCKET_RECV_ERROR)
|
||||||
|
{
|
||||||
|
std::cerr
|
||||||
|
<< __func__ << ": Receive error during command for device "
|
||||||
|
<< context->device.discoveredDevice.deviceIdentifier
|
||||||
|
<< std::endl;
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Result must have been RECV_SUCCESS state if we reach here
|
||||||
|
if (context->bytesReceived
|
||||||
|
< (ssize_t)sizeof(livoxProto1::comms::SamplingResponse))
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Response of size "
|
||||||
|
<< context->bytesReceived
|
||||||
|
<< " is too small for sampling response (expected "
|
||||||
|
<< sizeof(livoxProto1::comms::SamplingResponse) << ")"
|
||||||
|
<< std::endl;
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Parse response using protocol structure
|
||||||
|
livoxProto1::comms::SamplingResponse* response =
|
||||||
|
reinterpret_cast<livoxProto1::comms::SamplingResponse*>(
|
||||||
|
context->responseBuffer);
|
||||||
|
|
||||||
|
response->swapContentsToHostEndianness();
|
||||||
|
if (!response->sanityCheck())
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Invalid sampling response structure.\n";
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if response indicates success
|
||||||
|
if (response->command.cmd_set == 0x00 &&
|
||||||
|
response->command.cmd_id == 0x04 &&
|
||||||
|
response->ret_code == 0x00)
|
||||||
|
{
|
||||||
|
// Set the appropriate pcloud data active state based on command type
|
||||||
|
context->setPcloudDataActiveState();
|
||||||
|
context->callOriginalCallback(true);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we get here, the command failed
|
||||||
|
context->callOriginalCallbackWithFailure();
|
||||||
|
}
|
||||||
|
|
||||||
|
void cleanup()
|
||||||
|
{
|
||||||
|
timeoutTimer.cancel();
|
||||||
|
cleanupCmdResponseFdBoostWrapper();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pure virtual methods that derived classes must implement
|
||||||
|
virtual uint8_t getEnableFlag() const = 0;
|
||||||
|
virtual const char* getCommandName() const = 0;
|
||||||
|
virtual void setPcloudDataActiveState() = 0;
|
||||||
|
|
||||||
|
// Common sendCommand implementation
|
||||||
|
bool sendCommand()
|
||||||
|
{
|
||||||
|
// Create start/stop sampling message using protocol structure
|
||||||
|
livoxProto1::comms::StartStopSamplingMessage message;
|
||||||
|
|
||||||
|
// Set enable flag based on derived class implementation
|
||||||
|
message.enable = getEnableFlag();
|
||||||
|
|
||||||
|
// Calculate and set CRC32
|
||||||
|
message.footer.crc_32 = message.calculateCrc32();
|
||||||
|
message.swapContentsToProtocolEndianness();
|
||||||
|
|
||||||
|
struct sockaddr_in deviceAddr;
|
||||||
|
memset(&deviceAddr, 0, sizeof(deviceAddr));
|
||||||
|
deviceAddr.sin_family = AF_INET;
|
||||||
|
deviceAddr.sin_addr.s_addr =
|
||||||
|
inet_addr(device.discoveredDevice.ipAddr.c_str());
|
||||||
|
deviceAddr.sin_port = htons(65000); // Command port
|
||||||
|
|
||||||
|
// Send command directly (synchronous)
|
||||||
|
ssize_t bytesSent = sendto(
|
||||||
|
device.heartbeatFd,
|
||||||
|
&message, sizeof(message), 0,
|
||||||
|
(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));
|
||||||
|
|
||||||
|
if (bytesSent < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Failed to send " << getCommandName()
|
||||||
|
<< " command: " << strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
class Device::EnablePcloudDataReq
|
||||||
|
: public EnDisablePcloudDataReq<Device::enablePcloudDataReqCbFn>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend void Device::enablePcloudDataReq(
|
||||||
|
smo::Callback<Device::enablePcloudDataReqCbFn> callback);
|
||||||
|
|
||||||
|
EnablePcloudDataReq(
|
||||||
|
Device& dev,
|
||||||
|
smo::Callback<Device::enablePcloudDataReqCbFn> cb)
|
||||||
|
: EnDisablePcloudDataReq<Device::enablePcloudDataReqCbFn>(dev, std::move(cb))
|
||||||
|
{}
|
||||||
|
|
||||||
|
~EnablePcloudDataReq()
|
||||||
|
{
|
||||||
|
cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
uint8_t getEnableFlag() const override
|
||||||
|
{
|
||||||
|
return 0x01; // Start sampling
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* getCommandName() const override
|
||||||
|
{
|
||||||
|
return "enable pcloud data";
|
||||||
|
}
|
||||||
|
|
||||||
|
void setPcloudDataActiveState() override
|
||||||
|
{
|
||||||
|
device.pcloudDataActive.store(true);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
class Device::DisablePcloudDataReq
|
||||||
|
: public EnDisablePcloudDataReq<Device::disablePcloudDataReqCbFn>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend void Device::disablePcloudDataReq(
|
||||||
|
smo::Callback<Device::disablePcloudDataReqCbFn> callback);
|
||||||
|
|
||||||
|
DisablePcloudDataReq(
|
||||||
|
Device& dev,
|
||||||
|
smo::Callback<Device::disablePcloudDataReqCbFn> cb)
|
||||||
|
: EnDisablePcloudDataReq<Device::disablePcloudDataReqCbFn>(dev, std::move(cb))
|
||||||
|
{}
|
||||||
|
|
||||||
|
~DisablePcloudDataReq()
|
||||||
|
{
|
||||||
|
cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
uint8_t getEnableFlag() const override
|
||||||
|
{
|
||||||
|
return 0x00; // Stop sampling
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* getCommandName() const override
|
||||||
|
{
|
||||||
|
return "disable pcloud data";
|
||||||
|
}
|
||||||
|
|
||||||
|
void setPcloudDataActiveState() override
|
||||||
|
{
|
||||||
|
device.pcloudDataActive.store(false);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
void Device::enablePcloudDataReq(
|
||||||
|
smo::Callback<Device::enablePcloudDataReqCbFn> callback
|
||||||
|
)
|
||||||
|
{
|
||||||
|
auto request = std::make_shared<EnablePcloudDataReq>(
|
||||||
|
*this, std::move(callback));
|
||||||
|
|
||||||
|
// Check if heartbeat socket is available
|
||||||
|
if (heartbeatFd < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": No heartbeat socket available for device "
|
||||||
|
<< discoveredDevice.deviceIdentifier << std::endl;
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Setup socket for async operations
|
||||||
|
if (!request->setupSocket())
|
||||||
|
{
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set up the point cloud data socket for actual data reception
|
||||||
|
if (!setupPcloudDataSocket())
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Failed to set up point cloud data socket"
|
||||||
|
<< std::endl;
|
||||||
|
// Don't fail the command, but log the issue
|
||||||
|
}
|
||||||
|
|
||||||
|
// Send the start sampling command
|
||||||
|
if (!request->sendCommand())
|
||||||
|
{
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Setup async callbacks
|
||||||
|
request->setupAsyncCallbacks(request);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Device::disablePcloudDataReq(
|
||||||
|
smo::Callback<Device::disablePcloudDataReqCbFn> callback
|
||||||
|
)
|
||||||
|
{
|
||||||
|
auto request = std::make_shared<DisablePcloudDataReq>(
|
||||||
|
*this, std::move(callback));
|
||||||
|
|
||||||
|
// Check if heartbeat socket is available
|
||||||
|
if (heartbeatFd < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": No heartbeat socket available for device "
|
||||||
|
<< discoveredDevice.deviceIdentifier << std::endl;
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Unconditionally close the pcloud data socket early since there's no good
|
||||||
|
* reason to only close it if the command packet succeeds and ACKs. We want
|
||||||
|
* to stop receiving data immediately when disable is requested.
|
||||||
|
*/
|
||||||
|
cleanupPcloudDataSocket();
|
||||||
|
|
||||||
|
// Setup socket for async operations
|
||||||
|
if (!request->setupSocket())
|
||||||
|
{
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Send the stop sampling command
|
||||||
|
if (!request->sendCommand())
|
||||||
|
{
|
||||||
|
request->callOriginalCallbackWithFailure();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Setup async callbacks
|
||||||
|
request->setupAsyncCallbacks(request);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Device::setupPcloudDataSocket()
|
||||||
|
{
|
||||||
|
// RAII class to manage socket file descriptor
|
||||||
|
struct SocketRAII
|
||||||
|
{
|
||||||
|
int fd;
|
||||||
|
SocketRAII(int socketFd) : fd(socketFd) {}
|
||||||
|
~SocketRAII() { if (fd >= 0) close(fd); }
|
||||||
|
void commit() { fd = -1; } // Transfer ownership, prevent close
|
||||||
|
int getFd() const { return fd; }
|
||||||
|
bool isValid() const { return fd >= 0; }
|
||||||
|
};
|
||||||
|
|
||||||
|
// Create UDP socket for point cloud data reception
|
||||||
|
SocketRAII socketGuard(socket(AF_INET, SOCK_DGRAM, 0));
|
||||||
|
if (!socketGuard.isValid())
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Failed to create socket: "
|
||||||
|
<< strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set socket to non-blocking mode
|
||||||
|
int flags = fcntl(socketGuard.getFd(), F_GETFL, 0);
|
||||||
|
if (flags < 0 ||
|
||||||
|
fcntl(socketGuard.getFd(), F_SETFL, flags | O_NONBLOCK) < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Failed to set non-blocking mode: "
|
||||||
|
<< strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Bind to the data port (65001)
|
||||||
|
struct sockaddr_in localAddr;
|
||||||
|
memset(&localAddr, 0, sizeof(localAddr));
|
||||||
|
localAddr.sin_family = AF_INET;
|
||||||
|
localAddr.sin_addr.s_addr = INADDR_ANY;
|
||||||
|
localAddr.sin_port = htons(65001); // Data port
|
||||||
|
|
||||||
|
if (bind(
|
||||||
|
socketGuard.getFd(), (struct sockaddr *)&localAddr,
|
||||||
|
sizeof(localAddr)) < 0)
|
||||||
|
{
|
||||||
|
std::cerr << __func__ << ": Failed to bind to data port: "
|
||||||
|
<< strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create boost wrapper for async operations
|
||||||
|
pcloudDataSocketDesc =
|
||||||
|
std::make_unique<boost::asio::posix::stream_descriptor>(
|
||||||
|
componentThread->getIoService(), socketGuard.getFd());
|
||||||
|
|
||||||
|
pcloudDataFd = socketGuard.getFd();
|
||||||
|
// Transfer ownership, prevent auto-close
|
||||||
|
socketGuard.commit();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Device::cleanupPcloudDataSocket()
|
||||||
|
{
|
||||||
|
if (pcloudDataSocketDesc) {
|
||||||
|
pcloudDataSocketDesc->cancel();
|
||||||
|
pcloudDataSocketDesc.reset();
|
||||||
|
}
|
||||||
|
if (pcloudDataFd >= 0) {
|
||||||
|
close(pcloudDataFd);
|
||||||
|
pcloudDataFd = -1;
|
||||||
|
}
|
||||||
|
pcloudDataActive.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace livoxProto1
|
} // namespace livoxProto1
|
||||||
|
|||||||
@@ -12,6 +12,7 @@
|
|||||||
#include <arpa/inet.h>
|
#include <arpa/inet.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <boost/asio/deadline_timer.hpp>
|
#include <boost/asio/deadline_timer.hpp>
|
||||||
|
#include <boost/asio/posix/stream_descriptor.hpp>
|
||||||
#include "protocol.h"
|
#include "protocol.h"
|
||||||
#include <callback.h>
|
#include <callback.h>
|
||||||
|
|
||||||
@@ -95,6 +96,8 @@ private:
|
|||||||
class ConnectByDeviceIdentifierReq;
|
class ConnectByDeviceIdentifierReq;
|
||||||
class ExecuteHandshakeReq;
|
class ExecuteHandshakeReq;
|
||||||
class DisconnectReq;
|
class DisconnectReq;
|
||||||
|
class EnablePcloudDataReq;
|
||||||
|
class DisablePcloudDataReq;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
// Utility methods
|
// Utility methods
|
||||||
@@ -110,6 +113,8 @@ public:
|
|||||||
connectByDeviceIdentifierReqCbFn;
|
connectByDeviceIdentifierReqCbFn;
|
||||||
typedef std::function<void(bool success, int fd)> executeHandshakeReqCbFn;
|
typedef std::function<void(bool success, int fd)> executeHandshakeReqCbFn;
|
||||||
typedef std::function<void(bool success)> disconnectReqCbFn;
|
typedef std::function<void(bool success)> disconnectReqCbFn;
|
||||||
|
typedef std::function<void(bool success)> enablePcloudDataReqCbFn;
|
||||||
|
typedef std::function<void(bool success)> disablePcloudDataReqCbFn;
|
||||||
|
|
||||||
// Async connection methods
|
// Async connection methods
|
||||||
void connectReq(smo::Callback<connectReqCbFn> callback);
|
void connectReq(smo::Callback<connectReqCbFn> callback);
|
||||||
@@ -121,11 +126,24 @@ public:
|
|||||||
const std::string& deviceIP,
|
const std::string& deviceIP,
|
||||||
smo::Callback<executeHandshakeReqCbFn> callback);
|
smo::Callback<executeHandshakeReqCbFn> callback);
|
||||||
void disconnectReq(smo::Callback<disconnectReqCbFn> callback);
|
void disconnectReq(smo::Callback<disconnectReqCbFn> callback);
|
||||||
|
void enablePcloudDataReq(smo::Callback<enablePcloudDataReqCbFn> callback);
|
||||||
|
void disablePcloudDataReq(smo::Callback<disablePcloudDataReqCbFn> callback);
|
||||||
|
|
||||||
// Heartbeat state
|
// Heartbeat state
|
||||||
std::unique_ptr<boost::asio::deadline_timer> heartbeatTimer;
|
std::unique_ptr<boost::asio::deadline_timer> heartbeatTimer;
|
||||||
|
// FIXME: Might be useful to rename this to commandAndHeartbeatFd.
|
||||||
int heartbeatFd; // Socket file descriptor used for heartbeat
|
int heartbeatFd; // Socket file descriptor used for heartbeat
|
||||||
std::atomic<bool> heartbeatActive;
|
std::atomic<bool> heartbeatActive;
|
||||||
|
|
||||||
|
// Point cloud data state
|
||||||
|
std::unique_ptr<boost::asio::posix::stream_descriptor> pcloudDataSocketDesc;
|
||||||
|
std::atomic<bool> pcloudDataActive;
|
||||||
|
int pcloudDataFd; // Socket file descriptor for point cloud data reception
|
||||||
|
|
||||||
|
private:
|
||||||
|
// Point cloud data setup
|
||||||
|
bool setupPcloudDataSocket();
|
||||||
|
void cleanupPcloudDataSocket();
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace livoxProto1
|
} // namespace livoxProto1
|
||||||
|
|||||||
@@ -708,5 +708,98 @@ bool DisconnectMessage::validateCrc32() const
|
|||||||
return isValid;
|
return isValid;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// StartStopSamplingMessage methods
|
||||||
|
StartStopSamplingMessage::StartStopSamplingMessage()
|
||||||
|
{
|
||||||
|
// Initialize header
|
||||||
|
header.sof = 0xAA;
|
||||||
|
header.version = 1;
|
||||||
|
header.length = sizeof(StartStopSamplingMessage) - sizeof(Header) - sizeof(Footer);
|
||||||
|
header.cmd_type = 0x02; // MSG type
|
||||||
|
header.seq_num = 0; // Will be set by caller if needed
|
||||||
|
header.crc_16 = 0; // Will be calculated
|
||||||
|
|
||||||
|
// Initialize command
|
||||||
|
command.cmd_set = 0x00; // General command set
|
||||||
|
command.cmd_id = 0x04; // Sampling command ID
|
||||||
|
|
||||||
|
// Initialize data - enable flag will be set manually by caller
|
||||||
|
enable = 0x00; // Default to stop, caller will override
|
||||||
|
|
||||||
|
// Initialize footer
|
||||||
|
footer.crc_32 = 0; // Will be calculated
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t StartStopSamplingMessage::calculateCrc32() const
|
||||||
|
{
|
||||||
|
// Calculate CRC32 for the entire message excluding the footer CRC32 field
|
||||||
|
const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
|
||||||
|
size_t messageSize = sizeof(StartStopSamplingMessage) - sizeof(footer.crc_32);
|
||||||
|
|
||||||
|
return comms::calculateCrc32(messageData, messageSize);
|
||||||
|
}
|
||||||
|
|
||||||
|
void StartStopSamplingMessage::swapContentsToProtocolEndianness()
|
||||||
|
{
|
||||||
|
header.swapToProtocolEndianness();
|
||||||
|
command.swapToProtocolEndianness();
|
||||||
|
footer.swapToProtocolEndianness();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool StartStopSamplingMessage::sanityCheck() const
|
||||||
|
{
|
||||||
|
return header.sanityCheck() && command.sanityCheck() && footer.sanityCheck();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool StartStopSamplingMessage::validateCrc32() const
|
||||||
|
{
|
||||||
|
uint32_t calculatedCrc = calculateCrc32();
|
||||||
|
bool isValid = (calculatedCrc == footer.crc_32);
|
||||||
|
|
||||||
|
// Debug output only if validation fails
|
||||||
|
if (!isValid)
|
||||||
|
{
|
||||||
|
std::cout << "StartStopSamplingMessage CRC32 Debug: calculated=0x"
|
||||||
|
<< std::hex << calculatedCrc
|
||||||
|
<< ", received=0x" << footer.crc_32 << std::dec << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
return isValid;
|
||||||
|
}
|
||||||
|
|
||||||
|
// SamplingResponse methods
|
||||||
|
void SamplingResponse::swapContentsToHostEndianness()
|
||||||
|
{
|
||||||
|
header.swapToHostEndianness();
|
||||||
|
command.swapToHostEndianness();
|
||||||
|
footer.swapToHostEndianness();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool SamplingResponse::sanityCheck() const
|
||||||
|
{
|
||||||
|
return header.sanityCheck() && command.sanityCheck() && footer.sanityCheck();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool SamplingResponse::validateCrc32() const
|
||||||
|
{
|
||||||
|
// Calculate CRC32 for the entire message excluding the footer CRC32 field
|
||||||
|
const uint8_t* messageData = reinterpret_cast<const uint8_t*>(this);
|
||||||
|
size_t messageSize = sizeof(SamplingResponse) - sizeof(footer.crc_32);
|
||||||
|
|
||||||
|
uint32_t calculatedCrc = comms::calculateCrc32(messageData, messageSize);
|
||||||
|
bool isValid = (calculatedCrc == footer.crc_32);
|
||||||
|
|
||||||
|
// Debug output only if validation fails
|
||||||
|
if (!isValid)
|
||||||
|
{
|
||||||
|
std::cout << "SamplingResponse CRC32 Debug: calculated=0x"
|
||||||
|
<< std::hex << calculatedCrc
|
||||||
|
<< ", received=0x" << footer.crc_32 << std::dec << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
return isValid;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace comms
|
} // namespace comms
|
||||||
} // namespace livoxProto1
|
} // namespace livoxProto1
|
||||||
|
|||||||
@@ -246,6 +246,40 @@ struct DisconnectMessage
|
|||||||
bool validateCrc32() const;
|
bool validateCrc32() const;
|
||||||
} __attribute__((packed));
|
} __attribute__((packed));
|
||||||
|
|
||||||
|
/** EXPLANATION:
|
||||||
|
* Complete start/stop sampling command frame for enabling/disabling point cloud data from Livox devices.
|
||||||
|
* This is the complete wire format including header, command fields, data, and footer.
|
||||||
|
*/
|
||||||
|
struct StartStopSamplingMessage
|
||||||
|
{
|
||||||
|
Header header; // 0-8: Protocol frame header
|
||||||
|
Command command; // 9-10: Command identification
|
||||||
|
uint8_t enable; // 11: Enable flag (0x01 = Start, 0x00 = Stop)
|
||||||
|
Footer footer; // 12-15: Protocol frame footer
|
||||||
|
|
||||||
|
StartStopSamplingMessage();
|
||||||
|
uint32_t calculateCrc32() const;
|
||||||
|
void swapContentsToProtocolEndianness();
|
||||||
|
bool sanityCheck() const;
|
||||||
|
bool validateCrc32() const;
|
||||||
|
} __attribute__((packed));
|
||||||
|
|
||||||
|
/** EXPLANATION:
|
||||||
|
* Complete sampling response frame from Livox devices.
|
||||||
|
* This is the complete wire format including header, command fields, data, and footer.
|
||||||
|
*/
|
||||||
|
struct SamplingResponse
|
||||||
|
{
|
||||||
|
Header header; // 0-8: Protocol frame header
|
||||||
|
Command command; // 9-10: Command identification
|
||||||
|
uint8_t ret_code; // 11: Return Code (0x00 = Success, 0x01 = Fail)
|
||||||
|
Footer footer; // 12-15: Protocol frame footer
|
||||||
|
|
||||||
|
void swapContentsToHostEndianness();
|
||||||
|
bool sanityCheck() const;
|
||||||
|
bool validateCrc32() const;
|
||||||
|
} __attribute__((packed));
|
||||||
|
|
||||||
} // namespace comms
|
} // namespace comms
|
||||||
} // namespace livoxProto1
|
} // namespace livoxProto1
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user