226 lines
7.2 KiB
C++
226 lines
7.2 KiB
C++
#include "RobotProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include <cstring>
|
||
|
||
RobotProtocol::RobotProtocol()
|
||
: m_pModbusServer(nullptr)
|
||
, m_bServerRunning(false)
|
||
, m_nPort(502)
|
||
, m_robotStatus(STATUS_DISCONNECTED)
|
||
{
|
||
// 初始化坐标
|
||
memset(&m_robotCoordinate, 0, sizeof(m_robotCoordinate));
|
||
}
|
||
|
||
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) {
|
||
LOG_DEBUG("set callback func\n");
|
||
// 设置写线圈回调
|
||
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();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
// 设置初始状态
|
||
m_robotStatus = STATUS_CONNECTED;
|
||
|
||
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::SetRobotCoordinate(const RobotCoordinate& coordinate)
|
||
{
|
||
LOG_DEBUG("Set robot arm coordinates\n");
|
||
LOG_DEBUG("Coordinates: X=%.2f, Y=%.2f, Z=%.2f\n", coordinate.x, coordinate.y, coordinate.z);
|
||
LOG_DEBUG("Rotation: RX=%.2f, RY=%.2f, RZ=%.2f\n", coordinate.rx, coordinate.ry, coordinate.rz);
|
||
|
||
m_robotCoordinate = coordinate;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int RobotProtocol::GetRobotCoordinate(RobotCoordinate& coordinate)
|
||
{
|
||
coordinate = m_robotCoordinate;
|
||
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;
|
||
}
|
||
|
||
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);
|
||
|
||
// 处理工作信号线圈
|
||
uint16_t coilIndex = startAddress;
|
||
bool workSignal = (values[coilIndex / 8] >> (coilIndex % 8)) & 0x01;
|
||
|
||
LOG_INFO("Received work signal: %s\n", (workSignal ? "start work" : "stop work"));
|
||
|
||
// 触发工作信号回调
|
||
if (m_workSignalCallback) {
|
||
m_workSignalCallback(workSignal, coilIndex);
|
||
}
|
||
|
||
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;
|
||
|
||
IYModbusTCPServer::ErrorCode eResult = IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
switch (startAddress) {
|
||
case 0:
|
||
// 触发工作信号回调
|
||
if (m_workSignalCallback) {
|
||
if(!m_workSignalCallback(true, startAddress)){
|
||
eResult = IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
|
||
}
|
||
}
|
||
break;
|
||
default:
|
||
eResult = IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
|
||
break;
|
||
}
|
||
|
||
|
||
return eResult;
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress,uint16_t quantity, uint16_t* values)
|
||
{
|
||
LOG_DEBUG("Read holding registers - UnitID:%d, StartAddress:%d, Quantity:%d\n",
|
||
unitId, startAddress, quantity);
|
||
|
||
for (uint16_t i = 0; i < quantity; i++) {
|
||
uint16_t addr = startAddress + i;
|
||
|
||
if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
|
||
// X坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.x) + (addr - COORD_X_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
|
||
// Y坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.y) + (addr - COORD_Y_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
|
||
// Z坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.z) + (addr - COORD_Z_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
|
||
// RX坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rx) + (addr - COORD_RX_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
|
||
// RY坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.ry) + (addr - COORD_RY_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
|
||
// RZ坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rz) + (addr - COORD_RZ_ADDR), sizeof(uint16_t));
|
||
} else if (addr == STATUS_ADDR) {
|
||
// 状态寄存器
|
||
values[i] = static_cast<uint16_t>(m_robotStatus);
|
||
} else {
|
||
// 未定义的地址，返回0
|
||
values[i] = 0;
|
||
}
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|