261 lines
8.6 KiB
C++
261 lines
8.6 KiB
C++
#include "RobotProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include "IYModbusTCPServer.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)
|
||
{
|
||
// 数据格式: count + 每个目标的{id,status,width,depth}数据（每个数据占2个寄存器）
|
||
std::vector<uint16_t> data;
|
||
// 限制最大目标数量
|
||
size_t actualCount = multiTargetData.targets.size() > 5 ? 5 : multiTargetData.targets.size();
|
||
data.resize(actualCount * 16);
|
||
memset(data.data(), 0, data.size() * sizeof(uint16_t));
|
||
|
||
// 定义一个辅助函数来转换uint32_t到ModbusTCP寄存器顺序大端格式
|
||
auto uint32ToBigEndian = [&](uint16_t addr, uint32_t value) {
|
||
// ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
|
||
data[addr] = (value >> 16) & 0xFFFF; // 高16位寄存器
|
||
data[addr+1] = value & 0xFFFF; // 低16位寄存器
|
||
};
|
||
|
||
// 定义一个辅助函数来转换float到ModbusTCP寄存器顺序大端格式
|
||
auto floatToBigEndian = [&](uint16_t addr, float value) {
|
||
uint32_t intValue;
|
||
memcpy(&intValue, &value, sizeof(float));
|
||
// ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
|
||
data[addr] = (intValue >> 16) & 0xFFFF; // 高16位寄存器
|
||
data[addr+1] = intValue & 0xFFFF; // 低16位寄存器
|
||
};
|
||
|
||
// 定义一个辅助函数来转换uint16_t到ModbusTCP寄存器
|
||
auto uint16ToBigEndian = [&](uint16_t addr, uint16_t value) {
|
||
data[addr] = value; // uint16_t直接存储
|
||
};
|
||
|
||
// 从第二个寄存器开始存储目标数据 count [{id,status,width,depth}, {id,status,width,depth}]
|
||
for (size_t i = 0; i < actualCount; i++) {
|
||
uint16_t addr = i * RESULT_OFFSET_ADDR;
|
||
const TargetResult& target = multiTargetData.targets[i];
|
||
|
||
// 按照 id, status, width, depth 的顺序存储每个目标的数据
|
||
uint32ToBigEndian(addr, target.id); // ID (2个寄存器)
|
||
uint16ToBigEndian(addr+2, target.status); // Status (1个寄存器)
|
||
floatToBigEndian(addr+3, target.width); // Width (2个寄存器)
|
||
floatToBigEndian(addr+5, target.depth); // Depth (2个寄存器)
|
||
}
|
||
|
||
m_pModbusServer->updateHoldingRegisters(DETECT_RESULT_ADDR, data);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void RobotProtocol::SetSystemControlCallback(const std::function<void(uint16_t)>& callback)
|
||
{
|
||
m_systemControlCallback = 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 ERR_CODE(DEV_NO_OPEN);
|
||
}
|
||
|
||
// 更新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_WORKING: statusStr = "工作中"; break;
|
||
case WORK_STATUS_BUSY: statusStr = "忙碌"; break;
|
||
default: statusStr = "未知状态"; break;
|
||
}
|
||
|
||
LOG_INFO("Work status updated to: %d (%s)\n", status, statusStr);
|
||
return SUCCESS;
|
||
}
|
||
|
||
int RobotProtocol::ClearDetectionData()
|
||
{
|
||
LOG_INFO("Clearing detection result data in Modbus registers\n");
|
||
|
||
if (!m_pModbusServer) {
|
||
LOG_ERROR("ModbusTCP server not initialized\n");
|
||
return ERR_CODE(DEV_NO_OPEN);
|
||
}
|
||
|
||
// 清空检测结果数据区域（地址1-80，共5个撕裂信息，每个16个寄存器）
|
||
// 总共需要清空 5 * 16 = 80 个寄存器
|
||
std::vector<uint16_t> clearData(80, 0); // 创建80个值为0的寄存器
|
||
|
||
m_pModbusServer->updateHoldingRegisters(DETECT_RESULT_ADDR, clearData);
|
||
|
||
LOG_INFO("Detection result data cleared (registers 1-80)\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
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)
|
||
{
|
||
return IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values)
|
||
{
|
||
// 只允许写入地址0（系统状态寄存器）
|
||
if (startAddress != STATUS_ADDR) {
|
||
LOG_WARNING("Attempt to write to read-only address: %d\n", startAddress);
|
||
return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_ADDRESS;
|
||
}
|
||
|
||
// 只允许写入单个寄存器
|
||
if (quantity != 1) {
|
||
LOG_WARNING("Invalid quantity for status register write: %d\n", quantity);
|
||
return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
|
||
}
|
||
|
||
if (!values) {
|
||
LOG_ERROR("Null values pointer in write registers\n");
|
||
return IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
|
||
}
|
||
|
||
uint16_t command = values[0];
|
||
|
||
// 验证命令值是否有效（0-停止, 1-开始, 2-复位）
|
||
if (command > 2) {
|
||
LOG_WARNING("Invalid system control command: %d\n", command);
|
||
return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
|
||
}
|
||
|
||
LOG_INFO("Received system control command via ModbusTCP: %d\n", command);
|
||
|
||
// 调用系统控制回调
|
||
if (m_systemControlCallback) {
|
||
m_systemControlCallback(command);
|
||
LOG_DEBUG("System control callback executed for command: %d\n", command);
|
||
} else {
|
||
LOG_WARNING("System control callback not set\n");
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|
||
|
||
void RobotProtocol::OnModbusTCPConnectionChanged(bool connected)
|
||
{
|
||
|
||
}
|