255 lines
8.2 KiB
C++
255 lines
8.2 KiB
C++
#include "RobotProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include <cstring>
|
||
#include <algorithm>
|
||
|
||
RobotProtocol::RobotProtocol()
|
||
: m_pModbusServer(nullptr)
|
||
, m_bServerRunning(false)
|
||
, m_nPort(502)
|
||
, m_robotStatus(STATUS_DISCONNECTED)
|
||
{
|
||
}
|
||
|
||
RobotProtocol::~RobotProtocol()
|
||
{
|
||
Deinitialize();
|
||
}
|
||
|
||
int RobotProtocol::Initialize(uint16_t port)
|
||
{
|
||
if (m_bServerRunning) {
|
||
LOG_WARNING("Server is already running\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
m_nPort = port;
|
||
|
||
// 创建ModbusTCP服务器实例
|
||
bool bRet = IYModbusTCPServer::CreateInstance(&m_pModbusServer);
|
||
LOG_DEBUG("Create ModbusTCP server %s \n", bRet ? "success" : "failed");
|
||
m_bServerRunning = bRet;
|
||
|
||
// 设置ModbusTCP回调函数
|
||
if (m_pModbusServer) {
|
||
// 设置写线圈回调
|
||
m_pModbusServer->setWriteCoilsCallback([this](uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint8_t* values) {
|
||
return this->OnWriteCoils(unitId, startAddress, quantity, values);
|
||
});
|
||
|
||
// 设置写寄存器回调
|
||
m_pModbusServer->setWriteRegistersCallback([this](uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values) {
|
||
return this->OnWriteRegisters(unitId, startAddress, quantity, values);
|
||
});
|
||
|
||
// 设置连接状态回调
|
||
m_pModbusServer->setConnectionStatusCallback([this](bool connected) {
|
||
this->OnModbusTCPConnectionChanged(connected);
|
||
});
|
||
}
|
||
|
||
int nRet = m_pModbusServer->start(m_nPort);
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
// 设置初始状态
|
||
m_robotStatus = STATUS_CONNECTED;
|
||
|
||
// 设置初始工作状态为空闲
|
||
SetWorkStatus(WORK_STATUS_IDLE);
|
||
|
||
LOG_INFO("ModbusTCP service initialization completed\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
void RobotProtocol::Deinitialize()
|
||
{
|
||
LOG_DEBUG("Stop ModbusTCP service\n");
|
||
|
||
// 停止ModbusTCP服务器
|
||
StopModbusTCPServer();
|
||
|
||
// 重置状态
|
||
m_robotStatus = STATUS_DISCONNECTED;
|
||
m_bServerRunning = false;
|
||
|
||
LOG_INFO("ModbusTCP service stopped\n");
|
||
}
|
||
|
||
int RobotProtocol::SetMultiTargetData(const MultiTargetData& multiTargetData, uint16_t cameraId)
|
||
{
|
||
LOG_DEBUG("Set multi-target data, count: %d, camera ID: %d\n", multiTargetData.count, cameraId);
|
||
|
||
// 验证相机ID有效性（从1开始编号：1，2，...）
|
||
if (cameraId < 1 || cameraId > 2) {
|
||
LOG_ERROR("Invalid camera ID: %d, valid range is 1-2\n", cameraId);
|
||
return -1;
|
||
}
|
||
|
||
// 数据格式: count + 每个目标的{x,y,z,rz}坐标（每个坐标占2个寄存器）
|
||
std::vector<uint16_t> data;
|
||
// 限制最大目标数量
|
||
uint16_t actualCount = multiTargetData.count > 5 ? 5 : multiTargetData.count;
|
||
data.resize(actualCount * 8 + 1);
|
||
|
||
// 根据相机ID确定起始地址（cameraId: 1->COORD_ONE, 2->COORD_TWO）
|
||
uint16_t startAddr = (cameraId == 1) ? COORD_ONE_START_ADDR : COORD_TWO_START_ADDR;
|
||
|
||
// 设置目标数量
|
||
data[0] = actualCount;
|
||
|
||
// 从第二个寄存器开始存储坐标数据 count [{x,y,z,rz}, {x,y,z,rz}]
|
||
uint16_t dataIndex = 2;
|
||
|
||
// 定义一个辅助函数来转换float到ModbusTCP寄存器顺序大端格式
|
||
auto floatToBigEndian = [&](float value) {
|
||
uint32_t intValue;
|
||
memcpy(&intValue, &value, sizeof(float));
|
||
// ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
|
||
// 每个寄存器内部保持主机字节序
|
||
data[dataIndex++] = (intValue >> 16) & 0xFFFF; // 高16位寄存器
|
||
data[dataIndex++] = intValue & 0xFFFF; // 低16位寄存器
|
||
};
|
||
|
||
for (uint16_t i = 0; i < actualCount; i++) {
|
||
const TargetPosition& target = multiTargetData.targets[i];
|
||
|
||
// 按照 x, y, z, rz 的顺序存储每个目标的坐标
|
||
floatToBigEndian(target.x); // X坐标 (2个寄存器)
|
||
floatToBigEndian(target.y); // Y坐标 (2个寄存器)
|
||
floatToBigEndian(target.z); // Z坐标 (2个寄存器)
|
||
floatToBigEndian(target.rz); // RZ坐标 (2个寄存器)
|
||
}
|
||
|
||
m_pModbusServer->updateHoldingRegisters(startAddr, data);
|
||
|
||
return 0;
|
||
}
|
||
|
||
RobotProtocol::RobotStatus RobotProtocol::GetDetectionStatus() const
|
||
{
|
||
return m_robotStatus;
|
||
}
|
||
|
||
void RobotProtocol::SetConnectionCallback(const ConnectionCallback& callback)
|
||
{
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void RobotProtocol::SetWorkSignalCallback(const WorkSignalCallback& callback)
|
||
{
|
||
m_workSignalCallback = callback;
|
||
}
|
||
|
||
bool RobotProtocol::IsRunning() const
|
||
{
|
||
return m_bServerRunning;
|
||
}
|
||
|
||
int RobotProtocol::SetWorkStatus(uint16_t status)
|
||
{
|
||
LOG_DEBUG("Set work status to: %d\n", status);
|
||
|
||
if (!m_pModbusServer) {
|
||
LOG_ERROR("ModbusTCP server not initialized\n");
|
||
return -1;
|
||
}
|
||
|
||
// 更新STATUS_ADDR地址的寄存器值
|
||
std::vector<uint16_t> statusData;
|
||
statusData.push_back(status);
|
||
|
||
m_pModbusServer->updateHoldingRegisters(STATUS_ADDR, statusData);
|
||
|
||
const char* statusStr = "";
|
||
switch(status) {
|
||
case WORK_STATUS_IDLE: statusStr = "空闲"; break;
|
||
case WORK_STATUS_CAMERA1_DONE: statusStr = "相机1工作完成"; break;
|
||
case WORK_STATUS_CAMERA2_DONE: statusStr = "相机2工作完成"; break;
|
||
case WORK_STATUS_BUSY: statusStr = "忙碌"; break;
|
||
default: statusStr = "未知状态"; break;
|
||
}
|
||
|
||
LOG_INFO("Work status updated to: %d (%s)\n", status, statusStr);
|
||
return 0;
|
||
}
|
||
|
||
void RobotProtocol::StopModbusTCPServer()
|
||
{
|
||
LOG_DEBUG("Stop ModbusTCP server\n");
|
||
|
||
if (m_pModbusServer) {
|
||
// 释放资源
|
||
delete m_pModbusServer;
|
||
m_pModbusServer = nullptr;
|
||
}
|
||
|
||
m_bServerRunning = false;
|
||
LOG_INFO("ModbusTCP server stopped\n");
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint8_t* values)
|
||
{
|
||
LOG_DEBUG("Write coils - UnitID:%d, StartAddress:%d, Quantity:%d\n", unitId, startAddress, quantity);
|
||
|
||
// 处理工作信号线圈
|
||
if (startAddress == WORK_SIGNAL_ADDR) {
|
||
bool workSignal = (values[0] >> 0) & 0x01;
|
||
|
||
LOG_INFO("Received work signal: %s (coil address: %d)\n",
|
||
(workSignal ? "start work" : "stop work"), startAddress);
|
||
|
||
// 触发工作信号回调（使用相机ID 1作为默认值）
|
||
if (m_workSignalCallback) {
|
||
m_workSignalCallback(workSignal, 1); // 默认相机ID为1
|
||
}
|
||
} else {
|
||
LOG_WARNING("Unknown coil address: %d\n", startAddress);
|
||
return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_ADDRESS;
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values)
|
||
{
|
||
LOG_DEBUG("Write registers - UnitID:%d, StartAddress:%d, Quantity:%d\n", unitId, startAddress, quantity);
|
||
|
||
// 处理坐标寄存器更新
|
||
if(quantity > 1) return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
|
||
|
||
if(nullptr == m_workSignalCallback) return IYModbusTCPServer::ErrorCode::GATEWAY_TARGET_FAILED;
|
||
|
||
IYModbusTCPServer::ErrorCode eResult = IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
|
||
switch (startAddress) {
|
||
case WORK_SIGNAL_ADDR:
|
||
// 解析工作信号寄存器
|
||
if(!m_workSignalCallback(true, (int)values[0])){
|
||
eResult = IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
|
||
}
|
||
break;
|
||
default:
|
||
eResult = IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
|
||
break;
|
||
}
|
||
|
||
return eResult;
|
||
}
|
||
|
||
void RobotProtocol::OnModbusTCPConnectionChanged(bool connected)
|
||
{
|
||
if (connected) {
|
||
LOG_INFO("ModbusTCP connection established\n");
|
||
m_robotStatus = STATUS_CONNECTED;
|
||
} else {
|
||
LOG_INFO("ModbusTCP connection lost\n");
|
||
m_robotStatus = STATUS_DISCONNECTED;
|
||
}
|
||
|
||
// 触发上层连接状态回调
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(connected);
|
||
}
|
||
}
|