diff --git a/GrabBagApp/Presenter/Inc/GrabBagPresenter.h b/GrabBagApp/Presenter/Inc/GrabBagPresenter.h index 2760d56..78dc799 100644 --- a/GrabBagApp/Presenter/Inc/GrabBagPresenter.h +++ b/GrabBagApp/Presenter/Inc/GrabBagPresenter.h @@ -50,7 +50,7 @@ private: // 机械臂协议回调函数 void OnRobotConnectionChanged(bool connected); - void OnRobotWorkSignal(bool startWork, int cameraIndex); + bool OnRobotWorkSignal(bool startWork, int cameraIndex); // 连接状态检查和更新 void CheckAndUpdateWorkStatus(); diff --git a/GrabBagApp/Presenter/Inc/RobotProtocol.h b/GrabBagApp/Presenter/Inc/RobotProtocol.h index 8d26076..1239f73 100644 --- a/GrabBagApp/Presenter/Inc/RobotProtocol.h +++ b/GrabBagApp/Presenter/Inc/RobotProtocol.h @@ -46,7 +46,7 @@ public: * @param startWork true-开始工作，false-停止工作 * @param cameraIndex 相机索引，-1表示所有相机，0/1/2...表示特定相机 */ - using WorkSignalCallback = std::function; + using WorkSignalCallback = std::function; public: RobotProtocol(); diff --git a/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp b/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp index 3345df9..f80b366 100644 --- a/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp +++ b/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp @@ -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) diff --git a/GrabBagApp/Presenter/Src/RobotProtocol.cpp b/GrabBagApp/Presenter/Src/RobotProtocol.cpp index 30acd10..79914c4 100644 --- a/GrabBagApp/Presenter/Src/RobotProtocol.cpp +++ b/GrabBagApp/Presenter/Src/RobotProtocol.cpp @@ -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; - - 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(&newCoordinate.x) + (addr - COORD_X_ADDR), &value, sizeof(uint16_t)); - } else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) { - memcpy(reinterpret_cast(&newCoordinate.y) + (addr - COORD_Y_ADDR), &value, sizeof(uint16_t)); - } else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) { - memcpy(reinterpret_cast(&newCoordinate.z) + (addr - COORD_Z_ADDR), &value, sizeof(uint16_t)); - } else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) { - memcpy(reinterpret_cast(&newCoordinate.rx) + (addr - COORD_RX_ADDR), &value, sizeof(uint16_t)); - } else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) { - memcpy(reinterpret_cast(&newCoordinate.ry) + (addr - COORD_RY_ADDR), &value, sizeof(uint16_t)); - } else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) { - memcpy(reinterpret_cast(&newCoordinate.rz) + (addr - COORD_RZ_ADDR), &value, sizeof(uint16_t)); + 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; } } - - // 更新坐标 - 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); diff --git a/GrabBagApp/mainwindow.cpp b/GrabBagApp/mainwindow.cpp index ae88795..7c4988e 100644 --- a/GrabBagApp/mainwindow.cpp +++ b/GrabBagApp/mainwindow.cpp @@ -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,8 +316,95 @@ void MainWindow::updateDetectionResultDisplay(const DetectionResult& result) addDetectionResult(result); } +void MainWindow::updateDetectionLog(const QString& message) +{ + // 在UI线程中更新detect_log控件（QListView） + if (!m_logModel) return ; -void MainWindow::on_action_camera_1_triggered() + // 添加时间戳（包含年月日） + 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::clearDetectionLogUI() +{ + // 在UI线程中清空检测日志 + if (m_logModel) { + m_logModel->setStringList(QStringList()); + } +} + +void MainWindow::on_action_tool_debug_data_triggered() +{ + // 打开文件选择对话框 + QString fileName = QFileDialog::getOpenFileName( + this, + tr("选择调试数据文件"), + QString(), + tr("激光数据文件 (*.txt);;所有文件 (*.*)") + ); + + if (fileName.isEmpty()) { + // 用户取消了文件选择 + return; + } + + // 检查Presenter是否已初始化 + if (!m_presenter) { + QMessageBox::warning(this, tr("错误"), tr("系统未正确初始化！")); + return; + } + + // 清空检测日志，开始新的检测 + clearDetectionLog(); + + std::thread t([this, fileName]() { + updateStatusLog(tr("正在加载调试数据文件: %1").arg(fileName)); + int result = m_presenter->LoadDebugDataAndDetect(fileName.toStdString()); + + if (result == 0) { + updateStatusLog(tr("调试数据加载和检测成功")); + } else { + QString errorMsg = tr("调试数据加载失败，错误码: %1").arg(result); + updateStatusLog(errorMsg); + QMessageBox::warning(this, tr("错误"), errorMsg); + } + }); + t.detach(); +} + +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_btn_camera_clicked() { IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(0); if(nullptr == pDevice){ @@ -382,88 +417,15 @@ void MainWindow::on_action_camera_1_triggered() ui_dialogCamera->show(); } - -void MainWindow::on_action_camera_2_triggered() +void MainWindow::on_btn_algo_config_clicked() { - IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(1); - if(nullptr == pDevice){ - QMessageBox::warning(this, "设备错误", "相机设备未正确初始化！"); - return; + if(nullptr == ui_dialogConfig){ + ui_dialogConfig = new DialogConfig; } - if(nullptr == ui_dialogCamera){ - ui_dialogCamera = new DialogCamera(pDevice, this); - } - ui_dialogCamera->show(); + ui_dialogConfig->show(); } - -void MainWindow::on_action_tool_debug_data_triggered() +void MainWindow::on_btn_camera_levelling_clicked() { - // 打开文件选择对话框 - QString fileName = QFileDialog::getOpenFileName( - this, - tr("选择调试数据文件"), - QString(), - tr("激光数据文件 (*.txt);;所有文件 (*.*)") - ); - - if (fileName.isEmpty()) { - // 用户取消了文件选择 - return; - } - - // 检查Presenter是否已初始化 - if (!m_presenter) { - QMessageBox::warning(this, tr("错误"), tr("系统未正确初始化！")); - return; - } - - // 清空检测日志，开始新的检测 - clearDetectionLog(); - - std::thread t([this, fileName]() { - updateStatusLog(tr("正在加载调试数据文件: %1").arg(fileName)); - int result = m_presenter->LoadDebugDataAndDetect(fileName.toStdString()); - - if (result == 0) { - updateStatusLog(tr("调试数据加载和检测成功")); - } else { - QString errorMsg = tr("调试数据加载失败，错误码: %1").arg(result); - updateStatusLog(errorMsg); - QMessageBox::warning(this, tr("错误"), errorMsg); - } - }); - t.detach(); -} -void MainWindow::updateDetectionLog(const QString& message) -{ - // 在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); - - // 获取当前数据 - 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::clearDetectionLogUI() -{ - // 在UI线程中清空检测日志 - if (m_logModel) { - m_logModel->setStringList(QStringList()); - } -} - diff --git a/GrabBagApp/mainwindow.h b/GrabBagApp/mainwindow.h index 8670fc9..7529571 100644 --- a/GrabBagApp/mainwindow.h +++ b/GrabBagApp/mainwindow.h @@ -53,16 +53,7 @@ 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; diff --git a/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp b/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp index d84c941..9fe326c 100644 --- a/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp +++ b/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp @@ -281,59 +281,67 @@ void ModbusTCPServer::removeClient(int socket) void ModbusTCPServer::processModbusRequest(std::shared_ptr client, const uint8_t* query, int queryLength) { - // 处理请求并发送响应 - std::lock_guard 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 - 最小长度检查 + // 解析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 registerValues; + std::vector coilValues; + + // 基本长度检查 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 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 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 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 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 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 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)