主要调通了modbus协议,页面工作日志显示内容
This commit is contained in:
parent
84b39dc16d
commit
43cfe03d1a
@ -50,7 +50,7 @@ private:
|
||||
|
||||
// 机械臂协议回调函数
|
||||
void OnRobotConnectionChanged(bool connected);
|
||||
void OnRobotWorkSignal(bool startWork, int cameraIndex);
|
||||
bool OnRobotWorkSignal(bool startWork, int cameraIndex);
|
||||
|
||||
// 连接状态检查和更新
|
||||
void CheckAndUpdateWorkStatus();
|
||||
|
||||
@ -46,7 +46,7 @@ public:
|
||||
* @param startWork true-开始工作,false-停止工作
|
||||
* @param cameraIndex 相机索引,-1表示所有相机,0/1/2...表示特定相机
|
||||
*/
|
||||
using WorkSignalCallback = std::function<void(bool startWork, int cameraIndex)>;
|
||||
using WorkSignalCallback = std::function<bool(bool startWork, int cameraIndex)>;
|
||||
|
||||
public:
|
||||
RobotProtocol();
|
||||
|
||||
@ -203,7 +203,7 @@ int GrabBagPresenter::InitRobotProtocol()
|
||||
|
||||
// 设置工作信号回调
|
||||
m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
|
||||
this->OnRobotWorkSignal(startWork, cameraIndex);
|
||||
return this->OnRobotWorkSignal(startWork, cameraIndex);
|
||||
});
|
||||
|
||||
|
||||
@ -307,33 +307,28 @@ void GrabBagPresenter::OnRobotConnectionChanged(bool connected)
|
||||
CheckAndUpdateWorkStatus();
|
||||
}
|
||||
|
||||
void GrabBagPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
|
||||
bool GrabBagPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
|
||||
{
|
||||
cameraIndex += 1;
|
||||
LOG_INFO("Received robot work signal: %s for camera index: %d\n", (startWork ? "start work" : "stop work"), cameraIndex);
|
||||
|
||||
if (nullptr == m_pStatus) {
|
||||
return;
|
||||
}
|
||||
int nRet = SUCCESS;
|
||||
|
||||
if (startWork) {
|
||||
QString message;
|
||||
if (cameraIndex == -1) {
|
||||
message = "收到开始工作信号,启动所有相机检测";
|
||||
} else {
|
||||
message = QString("收到开始工作信号,启动相机 %1 检测").arg(cameraIndex + 1);
|
||||
QString message = QString("收到开始工作信号,启动相机 %1 检测").arg(cameraIndex);
|
||||
if (nullptr != m_pStatus) {
|
||||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||||
}
|
||||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||||
StartDetection(cameraIndex);
|
||||
nRet = StartDetection(cameraIndex + 1);
|
||||
} else {
|
||||
QString message;
|
||||
if (cameraIndex == -1) {
|
||||
message = "收到停止工作信号,停止所有相机检测";
|
||||
} else {
|
||||
message = QString("收到停止工作信号,停止相机 %1 检测").arg(cameraIndex + 1);
|
||||
QString message = QString("收到停止工作信号,停止相机 %1 检测").arg(cameraIndex);
|
||||
if (nullptr != m_pStatus) {
|
||||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||||
}
|
||||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||||
StopDetection();
|
||||
nRet = StopDetection();
|
||||
}
|
||||
|
||||
return nRet == SUCCESS;
|
||||
}
|
||||
|
||||
void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
|
||||
|
||||
@ -126,8 +126,7 @@ void RobotProtocol::StopModbusTCPServer()
|
||||
LOG_INFO("ModbusTCP server stopped\n");
|
||||
}
|
||||
|
||||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress,
|
||||
uint16_t quantity, const uint8_t* values)
|
||||
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);
|
||||
|
||||
@ -139,50 +138,39 @@ IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_
|
||||
|
||||
// 触发工作信号回调
|
||||
if (m_workSignalCallback) {
|
||||
m_workSignalCallback(coilIndex, workSignal);
|
||||
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)
|
||||
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 (startAddress >= COORD_X_ADDR && startAddress < STATUS_ADDR) {
|
||||
RobotCoordinate newCoordinate = m_robotCoordinate;
|
||||
if(quantity > 1) return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
|
||||
|
||||
for (uint16_t i = 0; i < quantity; i++) {
|
||||
uint16_t addr = startAddress + i;
|
||||
uint16_t value = values[i];
|
||||
|
||||
// 根据地址更新相应的坐标分量
|
||||
if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.x) + (addr - COORD_X_ADDR), &value, sizeof(uint16_t));
|
||||
} else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.y) + (addr - COORD_Y_ADDR), &value, sizeof(uint16_t));
|
||||
} else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.z) + (addr - COORD_Z_ADDR), &value, sizeof(uint16_t));
|
||||
} else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rx) + (addr - COORD_RX_ADDR), &value, sizeof(uint16_t));
|
||||
} else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.ry) + (addr - COORD_RY_ADDR), &value, sizeof(uint16_t));
|
||||
} else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
|
||||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rz) + (addr - COORD_RZ_ADDR), &value, sizeof(uint16_t));
|
||||
IYModbusTCPServer::ErrorCode eResult = IYModbusTCPServer::ErrorCode::SUCCESS;
|
||||
switch (startAddress) {
|
||||
case 0:
|
||||
// 触发工作信号回调
|
||||
if (m_workSignalCallback) {
|
||||
if(!m_workSignalCallback(true, startAddress)){
|
||||
eResult = IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
// 更新坐标
|
||||
SetRobotCoordinate(newCoordinate);
|
||||
break;
|
||||
default:
|
||||
eResult = IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
|
||||
break;
|
||||
}
|
||||
|
||||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||||
|
||||
return eResult;
|
||||
}
|
||||
|
||||
IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress,
|
||||
uint16_t quantity, uint16_t* values)
|
||||
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);
|
||||
|
||||
@ -43,11 +43,6 @@ MainWindow::MainWindow(QWidget *parent)
|
||||
|
||||
// 原有的状态指示器已删除,状态显示统一在devstatus中处理
|
||||
|
||||
// 连接菜单信号
|
||||
// connect(ui->menu_camera, &QMenu::aboutToShow, this, &MainWindow::on_menu_camera_triggered);
|
||||
// connect(ui->menu_config, &QMenu::aboutToShow, this, &MainWindow::on_menu_config_triggered);
|
||||
// connect(ui->menu_camera_adjust, &QMenu::aboutToShow, this, &MainWindow::on_menu_camera_adjust_triggered);
|
||||
|
||||
// 连接工作状态更新信号槽
|
||||
connect(this, &MainWindow::workStatusUpdateRequested, this, &MainWindow::updateWorkStatusLabel);
|
||||
|
||||
@ -212,53 +207,6 @@ void MainWindow::addDetectionResult(const DetectionResult& result)
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_camera_clicked()
|
||||
{
|
||||
// if(nullptr == ui_dialogCamera){
|
||||
// ui_dialogCamera = new DialogCamera;
|
||||
// }
|
||||
// ui_dialogCamera->show();
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_config_clicked()
|
||||
{
|
||||
if(nullptr == ui_dialogConfig){
|
||||
ui_dialogConfig = new DialogConfig;
|
||||
}
|
||||
ui_dialogConfig->show();
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_flow_clicked()
|
||||
{
|
||||
// 机械臂连接/断开
|
||||
static bool isConnected = false;
|
||||
isConnected = !isConnected;
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_start_clicked()
|
||||
{
|
||||
// 清空检测日志,开始新的检测
|
||||
clearDetectionLog();
|
||||
|
||||
// 使用Presenter启动检测
|
||||
if (m_presenter) {
|
||||
m_presenter->StartDetection();
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_stop_clicked()
|
||||
{
|
||||
if (m_presenter) {
|
||||
m_presenter->StopDetection();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_menu_camera_adjust_triggered()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
// 状态更新槽函数
|
||||
void MainWindow::OnStatusUpdate(const std::string& statusMessage)
|
||||
{
|
||||
@ -368,35 +316,37 @@ void MainWindow::updateDetectionResultDisplay(const DetectionResult& result)
|
||||
addDetectionResult(result);
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_action_camera_1_triggered()
|
||||
void MainWindow::updateDetectionLog(const QString& message)
|
||||
{
|
||||
IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(0);
|
||||
if(nullptr == pDevice){
|
||||
QMessageBox::warning(this, "设备错误", "相机设备未正确初始化!");
|
||||
return;
|
||||
}
|
||||
if(nullptr == ui_dialogCamera){
|
||||
ui_dialogCamera = new DialogCamera(pDevice, this);
|
||||
}
|
||||
ui_dialogCamera->show();
|
||||
// 在UI线程中更新detect_log控件(QListView)
|
||||
if (!m_logModel) return ;
|
||||
|
||||
// 添加时间戳(包含年月日)
|
||||
QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss");
|
||||
QString logEntry = QString("[%1] %2").arg(timestamp).arg(message);
|
||||
|
||||
// 获取当前数据
|
||||
QStringList logList = m_logModel->stringList();
|
||||
|
||||
// 添加新的日志条目
|
||||
logList.append(logEntry);
|
||||
|
||||
// 更新模型
|
||||
m_logModel->setStringList(logList);
|
||||
|
||||
// 自动滚动到最底部
|
||||
QModelIndex lastIndex = m_logModel->index(logList.size() - 1);
|
||||
ui->detect_log->scrollTo(lastIndex);
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_action_camera_2_triggered()
|
||||
void MainWindow::clearDetectionLogUI()
|
||||
{
|
||||
IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(1);
|
||||
if(nullptr == pDevice){
|
||||
QMessageBox::warning(this, "设备错误", "相机设备未正确初始化!");
|
||||
return;
|
||||
// 在UI线程中清空检测日志
|
||||
if (m_logModel) {
|
||||
m_logModel->setStringList(QStringList());
|
||||
}
|
||||
if(nullptr == ui_dialogCamera){
|
||||
ui_dialogCamera = new DialogCamera(pDevice, this);
|
||||
}
|
||||
ui_dialogCamera->show();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_action_tool_debug_data_triggered()
|
||||
{
|
||||
// 打开文件选择对话框
|
||||
@ -405,7 +355,7 @@ void MainWindow::on_action_tool_debug_data_triggered()
|
||||
tr("选择调试数据文件"),
|
||||
QString(),
|
||||
tr("激光数据文件 (*.txt);;所有文件 (*.*)")
|
||||
);
|
||||
);
|
||||
|
||||
if (fileName.isEmpty()) {
|
||||
// 用户取消了文件选择
|
||||
@ -436,34 +386,46 @@ void MainWindow::on_action_tool_debug_data_triggered()
|
||||
t.detach();
|
||||
}
|
||||
|
||||
void MainWindow::updateDetectionLog(const QString& message)
|
||||
void MainWindow::on_btn_start_clicked()
|
||||
{
|
||||
// 在UI线程中更新detect_log控件(QListView)
|
||||
if (m_logModel) {
|
||||
// 添加时间戳(包含年月日)
|
||||
QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss");
|
||||
QString logEntry = QString("[%1] %2").arg(timestamp).arg(message);
|
||||
// 清空检测日志,开始新的检测
|
||||
clearDetectionLog();
|
||||
|
||||
// 获取当前数据
|
||||
QStringList logList = m_logModel->stringList();
|
||||
|
||||
// 添加新的日志条目
|
||||
logList.append(logEntry);
|
||||
|
||||
// 更新模型
|
||||
m_logModel->setStringList(logList);
|
||||
|
||||
// 自动滚动到最底部
|
||||
QModelIndex lastIndex = m_logModel->index(logList.size() - 1);
|
||||
ui->detect_log->scrollTo(lastIndex);
|
||||
// 使用Presenter启动检测
|
||||
if (m_presenter) {
|
||||
m_presenter->StartDetection();
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::clearDetectionLogUI()
|
||||
void MainWindow::on_btn_stop_clicked()
|
||||
{
|
||||
// 在UI线程中清空检测日志
|
||||
if (m_logModel) {
|
||||
m_logModel->setStringList(QStringList());
|
||||
if (m_presenter) {
|
||||
m_presenter->StopDetection();
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_camera_clicked()
|
||||
{
|
||||
IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(0);
|
||||
if(nullptr == pDevice){
|
||||
QMessageBox::warning(this, "设备错误", "相机设备未正确初始化!");
|
||||
return;
|
||||
}
|
||||
if(nullptr == ui_dialogCamera){
|
||||
ui_dialogCamera = new DialogCamera(pDevice, this);
|
||||
}
|
||||
ui_dialogCamera->show();
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_algo_config_clicked()
|
||||
{
|
||||
if(nullptr == ui_dialogConfig){
|
||||
ui_dialogConfig = new DialogConfig;
|
||||
}
|
||||
ui_dialogConfig->show();
|
||||
}
|
||||
|
||||
void MainWindow::on_btn_camera_levelling_clicked()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
@ -53,15 +53,6 @@ signals:
|
||||
void logClearRequested();
|
||||
|
||||
private slots:
|
||||
// UI操作相关槽
|
||||
void on_btn_camera_clicked();
|
||||
void on_btn_config_clicked();
|
||||
void on_btn_flow_clicked();
|
||||
void on_btn_start_clicked();
|
||||
void on_btn_stop_clicked();
|
||||
// void on_menu_camera_triggered();
|
||||
// void on_menu_config_triggered();
|
||||
void on_menu_camera_adjust_triggered();
|
||||
|
||||
// 工作状态更新槽函数
|
||||
void updateWorkStatusLabel(WorkStatus status);
|
||||
@ -75,12 +66,18 @@ private slots:
|
||||
// 清空日志槽函数
|
||||
void clearDetectionLogUI();
|
||||
|
||||
void on_action_camera_1_triggered();
|
||||
|
||||
void on_action_camera_2_triggered();
|
||||
|
||||
void on_action_tool_debug_data_triggered();
|
||||
|
||||
// UI操作相关槽
|
||||
void on_btn_start_clicked();
|
||||
void on_btn_stop_clicked();
|
||||
|
||||
void on_btn_camera_clicked();
|
||||
|
||||
void on_btn_algo_config_clicked();
|
||||
|
||||
void on_btn_camera_levelling_clicked();
|
||||
|
||||
private:
|
||||
Ui::MainWindow *ui;
|
||||
DialogCamera* ui_dialogCamera = nullptr;
|
||||
|
||||
@ -281,59 +281,67 @@ void ModbusTCPServer::removeClient(int socket)
|
||||
|
||||
void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> client, const uint8_t* query, int queryLength)
|
||||
{
|
||||
// 处理请求并发送响应
|
||||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||||
int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping);
|
||||
// 使用modbus标准异常码,0表示成功
|
||||
uint8_t exceptionCode = 0;
|
||||
|
||||
if (responseRc == -1) {
|
||||
LOG_WARNING("Send response failed: %s\n", modbus_strerror(errno));
|
||||
throw std::runtime_error("发送响应失败");
|
||||
}
|
||||
// 解析ModbusTCP ADU的基本字段
|
||||
uint16_t transactionId = 0;
|
||||
uint16_t protocolId = 0;
|
||||
uint16_t length = 0;
|
||||
uint8_t unitId = 0;
|
||||
uint8_t function = 0;
|
||||
uint16_t address = 0;
|
||||
uint16_t value = 0;
|
||||
uint16_t quantity = 0;
|
||||
uint8_t byteCount = 0;
|
||||
std::vector<uint16_t> registerValues;
|
||||
std::vector<uint8_t> coilValues;
|
||||
|
||||
// 解析ModbusTCP ADU - 最小长度检查
|
||||
// 基本长度检查
|
||||
if (queryLength < 8) {
|
||||
LOG_DEBUG("Query too short: %d bytes\n", queryLength);
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
modbus_reply_exception(client->modbusCtx, query, exceptionCode);
|
||||
return;
|
||||
}
|
||||
|
||||
// ModbusTCP ADU格式: [Transaction ID(2)] [Protocol ID(2)] [Length(2)] [Unit ID(1)] [Function Code(1)] [Data...]
|
||||
uint16_t transactionId = (query[0] << 8) | query[1]; // Transaction ID
|
||||
uint16_t protocolId = (query[2] << 8) | query[3]; // Protocol ID (should be 0 for ModbusTCP)
|
||||
uint16_t length = (query[4] << 8) | query[5]; // Length field
|
||||
uint8_t unitId = query[6]; // Unit ID
|
||||
uint8_t function = query[7]; // Function Code
|
||||
|
||||
// 解析ADU头部
|
||||
transactionId = (query[0] << 8) | query[1];
|
||||
protocolId = (query[2] << 8) | query[3];
|
||||
length = (query[4] << 8) | query[5];
|
||||
unitId = query[6];
|
||||
function = query[7];
|
||||
|
||||
// 验证协议ID
|
||||
if (protocolId != 0) {
|
||||
LOG_DEBUG("Invalid protocol ID: %d\n", protocolId);
|
||||
return;
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
}
|
||||
|
||||
// 验证长度字段
|
||||
if (length != (queryLength - 6)) {
|
||||
LOG_DEBUG("Length mismatch: expected %d, got %d\n", length, queryLength - 6);
|
||||
else if (length != (queryLength - 6)) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
}
|
||||
|
||||
if(exceptionCode != 0) {
|
||||
modbus_reply_exception(client->modbusCtx, query, exceptionCode);
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_DEBUG("ModbusTCP request - TransID:%d, UnitID:%d, Function:0x%02X\n",
|
||||
transactionId, unitId, function);
|
||||
// 根据功能码验证数据格式并执行回调
|
||||
LOG_DEBUG("Modbus %02X - Trans:%d Unit:%d\n", function, transactionId, unitId);
|
||||
|
||||
// 根据功能码解析数据
|
||||
switch (function) {
|
||||
case MODBUS_FC_WRITE_SINGLE_COIL:
|
||||
{
|
||||
if (queryLength >= 12) {
|
||||
uint16_t address = (query[8] << 8) | query[9];
|
||||
uint16_t value = (query[10] << 8) | query[11];
|
||||
|
||||
LOG_DEBUG("Write single coil - Address:%d, Value:0x%x\n", address, value);
|
||||
if (queryLength < 12) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
address = (query[8] << 8) | query[9];
|
||||
value = (query[10] << 8) | query[11];
|
||||
|
||||
if (m_writeCoilsCallback) {
|
||||
uint8_t coilValue = (value == 0xFF00) ? 1 : 0;
|
||||
ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue);
|
||||
if (result != ErrorCode::SUCCESS) {
|
||||
LOG_DEBUG("Write coil callback returned error: %d\n", (int)result);
|
||||
}
|
||||
IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue);
|
||||
exceptionCode = (uint8_t)result;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -341,21 +349,21 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
|
||||
|
||||
case MODBUS_FC_WRITE_MULTIPLE_COILS:
|
||||
{
|
||||
if (queryLength >= 13) {
|
||||
uint16_t address = (query[8] << 8) | query[9];
|
||||
uint16_t quantity = (query[10] << 8) | query[11];
|
||||
uint8_t byteCount = query[12];
|
||||
if (queryLength < 13) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
address = (query[8] << 8) | query[9];
|
||||
quantity = (query[10] << 8) | query[11];
|
||||
byteCount = query[12];
|
||||
|
||||
if (queryLength >= (13 + byteCount)) {
|
||||
LOG_DEBUG("Write multiple coils - Address:%d, Quantity:%d, Bytes:%d\n",
|
||||
address, quantity, byteCount);
|
||||
if (queryLength < (13 + byteCount)) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
coilValues.assign(&query[13], &query[13 + byteCount]);
|
||||
|
||||
if (m_writeCoilsCallback) {
|
||||
const uint8_t* values = &query[13];
|
||||
ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, values);
|
||||
if (result != ErrorCode::SUCCESS) {
|
||||
LOG_DEBUG("Write coils callback returned error: %d\n", (int)result);
|
||||
}
|
||||
IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, coilValues.data());
|
||||
exceptionCode = (uint8_t)result;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -364,17 +372,15 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
|
||||
|
||||
case MODBUS_FC_WRITE_SINGLE_REGISTER:
|
||||
{
|
||||
if (queryLength >= 12) {
|
||||
uint16_t address = (query[8] << 8) | query[9];
|
||||
uint16_t value = (query[10] << 8) | query[11];
|
||||
|
||||
LOG_DEBUG("Write single register - Address:%d, Value:%d\n", address, value);
|
||||
if (queryLength < 12) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
address = (query[8] << 8) | query[9];
|
||||
value = (query[10] << 8) | query[11];
|
||||
|
||||
if (m_writeRegistersCallback) {
|
||||
ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value);
|
||||
if (result != ErrorCode::SUCCESS) {
|
||||
LOG_DEBUG("Write register callback returned error: %d\n", (int)result);
|
||||
}
|
||||
IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value);
|
||||
exceptionCode = (uint8_t)result;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -382,29 +388,25 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
|
||||
|
||||
case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
|
||||
{
|
||||
if (queryLength >= 13) {
|
||||
uint16_t address = (query[8] << 8) | query[9];
|
||||
uint16_t quantity = (query[10] << 8) | query[11];
|
||||
uint8_t byteCount = query[12];
|
||||
if (queryLength < 13) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
address = (query[8] << 8) | query[9];
|
||||
quantity = (query[10] << 8) | query[11];
|
||||
byteCount = query[12];
|
||||
|
||||
if (queryLength >= (13 + byteCount) && byteCount == (quantity * 2)) {
|
||||
LOG_DEBUG("Write multiple registers - Address:%d, Quantity:%d, Bytes:%d\n",
|
||||
address, quantity, byteCount);
|
||||
if (queryLength < (13 + byteCount) || byteCount != (quantity * 2)) {
|
||||
exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
|
||||
} else {
|
||||
registerValues.reserve(quantity);
|
||||
for (int i = 0; i < quantity; i++) {
|
||||
uint16_t regValue = (query[13 + i*2] << 8) | query[13 + i*2 + 1];
|
||||
registerValues.push_back(regValue);
|
||||
}
|
||||
|
||||
if (m_writeRegistersCallback) {
|
||||
// 解析寄存器值(注意字节序)
|
||||
std::vector<uint16_t> values;
|
||||
values.reserve(quantity);
|
||||
|
||||
for (int i = 0; i < quantity; i++) {
|
||||
uint16_t value = (query[13 + i*2] << 8) | query[13 + i*2 + 1];
|
||||
values.push_back(value);
|
||||
}
|
||||
|
||||
ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, values.data());
|
||||
if (result != ErrorCode::SUCCESS) {
|
||||
LOG_DEBUG("Write registers callback returned error: %d\n", (int)result);
|
||||
}
|
||||
IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, registerValues.data());
|
||||
exceptionCode = (uint8_t)result;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -412,9 +414,28 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
|
||||
break;
|
||||
|
||||
default:
|
||||
LOG_DEBUG("Unhandled function code: 0x%02X\n", function);
|
||||
// exceptionCode = MODBUS_EXCEPTION_ILLEGAL_FUNCTION;
|
||||
break;
|
||||
}
|
||||
|
||||
LOG_DEBUG("exceptionCode: %d\n", exceptionCode);
|
||||
|
||||
// 统一处理响应
|
||||
if (exceptionCode != 0) {
|
||||
modbus_reply_exception(client->modbusCtx, query, exceptionCode);
|
||||
return;
|
||||
}
|
||||
|
||||
// 发送正常响应
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||||
int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping);
|
||||
|
||||
if (responseRc == -1) {
|
||||
modbus_reply_exception(client->modbusCtx, query, MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModbusTCPServer::updateCoil(uint16_t address, bool value)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user