主要调通了modbus协议，页面工作日志显示内容

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();

GrabBagApp/Presenter/Inc/RobotProtocol.h

@@ -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();

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) {
+    int nRet = SUCCESS;
-        return;
-    }
     
     if (startWork) {
-        QString message;
+        QString message = QString("收到开始工作信号，启动相机 %1 检测").arg(cameraIndex);
-        if (cameraIndex == -1) {
+        if (nullptr != m_pStatus) {
-            message = "收到开始工作信号，启动所有相机检测";
-        } else {
-            message = QString("收到开始工作信号，启动相机 %1 检测").arg(cameraIndex + 1);
-        }
             m_pStatus->OnStatusUpdate(message.toStdString());
-        StartDetection(cameraIndex);
-    } else {
-        QString message;
-        if (cameraIndex == -1) {
-            message = "收到停止工作信号，停止所有相机检测";
-        } else {
-            message = QString("收到停止工作信号，停止相机 %1 检测").arg(cameraIndex + 1);
         }
+        nRet = StartDetection(cameraIndex + 1);
+    } else {
+        QString message = QString("收到停止工作信号，停止相机 %1 检测").arg(cameraIndex);
+        if (nullptr != m_pStatus) {
             m_pStatus->OnStatusUpdate(message.toStdString());
-        StopDetection();
         }
+        nRet = StopDetection();
+    }
+
+    return nRet == SUCCESS;
 }
 
 void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)

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, 
+IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint8_t* values)
-                                                        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, 
+IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values)
-                                                            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) {
+    if(quantity > 1) return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
-        RobotCoordinate newCoordinate = m_robotCoordinate;
 
-        for (uint16_t i = 0; i < quantity; i++) {
+    IYModbusTCPServer::ErrorCode eResult = IYModbusTCPServer::ErrorCode::SUCCESS;
-            uint16_t addr = startAddress + i;
+    switch (startAddress) {
-            uint16_t value = values[i];
+    case 0:
-            
+        // 触发工作信号回调
-            // 根据地址更新相应的坐标分量
+        if (m_workSignalCallback) {
-            if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
+            if(!m_workSignalCallback(true, startAddress)){
-                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.x) + (addr - COORD_X_ADDR), &value, sizeof(uint16_t));
+                eResult = IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
-            } 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));
             }
         }
-        
+        break;
-        // 更新坐标
+    default:
-        SetRobotCoordinate(newCoordinate);
+        eResult = IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
+        break;
     }
 
-    return IYModbusTCPServer::ErrorCode::SUCCESS;
+    
+    return eResult;
 }
 
-IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress, 
+IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress,uint16_t quantity, uint16_t* values)
-                                                                  uint16_t quantity, uint16_t* values)
 {
     LOG_DEBUG("Read holding registers - UnitID:%d, StartAddress:%d, Quantity:%d\n", 
               unitId, startAddress, quantity);

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,34 +316,36 @@ void MainWindow::updateDetectionResultDisplay(const DetectionResult& result)
     addDetectionResult(result);
 }
 
-
+void MainWindow::updateDetectionLog(const QString& message)
-void MainWindow::on_action_camera_1_triggered()
 {
-    IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(0);
+    // 在UI线程中更新detect_log控件（QListView）
-    if(nullptr == pDevice){
+    if (!m_logModel) return ;
-        QMessageBox::warning(this, "设备错误", "相机设备未正确初始化！");
+
-        return;
+    // 添加时间戳（包含年月日）
-    }
+    QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss");
-    if(nullptr == ui_dialogCamera){
+    QString logEntry = QString("[%1] %2").arg(timestamp).arg(message);
-        ui_dialogCamera = new DialogCamera(pDevice, this);
+
-    }
+    // 获取当前数据
-    ui_dialogCamera->show();
+    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()
-void MainWindow::on_action_camera_2_triggered()
 {
-    IVrEyeDevice* pDevice = m_presenter->GetEyeDevice(1);
+    // 在UI线程中清空检测日志
-    if(nullptr == pDevice){
+    if (m_logModel) {
-        QMessageBox::warning(this, "设备错误", "相机设备未正确初始化！");
+        m_logModel->setStringList(QStringList());
-        return;
     }
-    if(nullptr == ui_dialogCamera){
-        ui_dialogCamera = new DialogCamera(pDevice, this);
 }
-    ui_dialogCamera->show();
-}
-
 
 void MainWindow::on_action_tool_debug_data_triggered()
 {
@@ -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) {
+    clearDetectionLog();
-        // 添加时间戳（包含年月日）
-        QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss");
-        QString logEntry = QString("[%1] %2").arg(timestamp).arg(message);
 
-        // 获取当前数据
+    // 使用Presenter启动检测
-        QStringList logList = m_logModel->stringList();
+    if (m_presenter) {
-        
+        m_presenter->StartDetection();
-        // 添加新的日志条目
-        logList.append(logEntry);
-        
-        // 更新模型
-        m_logModel->setStringList(logList);
-        
-        // 自动滚动到最底部
-        QModelIndex lastIndex = m_logModel->index(logList.size() - 1);
-        ui->detect_log->scrollTo(lastIndex);
     }
 }
 
-void MainWindow::clearDetectionLogUI()
+void MainWindow::on_btn_stop_clicked()
 {
-    // 在UI线程中清空检测日志
+    if (m_presenter) {
-    if (m_logModel) {
+        m_presenter->StopDetection();
-        m_logModel->setStringList(QStringList());
     }
 }
 
+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()
+{
+
+}

GrabBagApp/mainwindow.h

@@ -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;

Module/ModbusTCPServer/Src/ModbusTCPServer.cpp

@@ -281,59 +281,67 @@ void ModbusTCPServer::removeClient(int socket)
 
 void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> client, const uint8_t* query, int queryLength)
 {
-    // 处理请求并发送响应
+    // 使用modbus标准异常码，0表示成功
-    std::lock_guard<std::mutex> lock(m_dataMutex);
+    uint8_t exceptionCode = 0;
-    int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping);
     
-    if (responseRc == -1) {
+    // 解析ModbusTCP ADU的基本字段
-        LOG_WARNING("Send response failed: %s\n", modbus_strerror(errno));
+    uint16_t transactionId = 0;
-        throw std::runtime_error("发送响应失败");
+    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
+    // 解析ADU头部
-    uint16_t protocolId = (query[2] << 8) | query[3];      // Protocol ID (should be 0 for ModbusTCP)
+    transactionId = (query[0] << 8) | query[1];
-    uint16_t length = (query[4] << 8) | query[5];          // Length field
+    protocolId = (query[2] << 8) | query[3];
-    uint8_t unitId = query[6];                             // Unit ID
+    length = (query[4] << 8) | query[5];
-    uint8_t function = query[7];                           // Function Code
+    unitId = query[6];
+    function = query[7];
     
     // 验证协议ID
     if (protocolId != 0) {
-        LOG_DEBUG("Invalid protocol ID: %d\n", protocolId);
+        exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-        return;
     }
-    
     // 验证长度字段
-    if (length != (queryLength - 6)) {
+    else if (length != (queryLength - 6)) {
-        LOG_DEBUG("Length mismatch: expected %d, got %d\n", 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) {
+                if (queryLength < 12) {
-                    uint16_t address = (query[8] << 8) | query[9];
+                    exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                    uint16_t value = (query[10] << 8) | query[11];
+                } else {
-                    
+                    address = (query[8] << 8) | query[9];
-                    LOG_DEBUG("Write single coil - Address:%d, Value:0x%x\n", address, value);
+                    value = (query[10] << 8) | query[11];
                     
                     if (m_writeCoilsCallback) {
                         uint8_t coilValue = (value == 0xFF00) ? 1 : 0;
-                        ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue);
+                        IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue);
-                        if (result != ErrorCode::SUCCESS) {
+                        exceptionCode = (uint8_t)result;
-                            LOG_DEBUG("Write coil callback returned error: %d\n", (int)result);
-                        }
                     }
                 }
             }
@@ -341,21 +349,21 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
             
         case MODBUS_FC_WRITE_MULTIPLE_COILS:
             {
-                if (queryLength >= 13) {
+                if (queryLength < 13) {
-                    uint16_t address = (query[8] << 8) | query[9];
+                    exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                    uint16_t quantity = (query[10] << 8) | query[11];
+                } else {
-                    uint8_t byteCount = query[12];
+                    address = (query[8] << 8) | query[9];
+                    quantity = (query[10] << 8) | query[11];
+                    byteCount = query[12];
                     
-                    if (queryLength >= (13 + byteCount)) {
+                    if (queryLength < (13 + byteCount)) {
-                        LOG_DEBUG("Write multiple coils - Address:%d, Quantity:%d, Bytes:%d\n", 
+                        exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                                  address, quantity, byteCount);
+                    } else {
+                        coilValues.assign(&query[13], &query[13 + byteCount]);
                         
                         if (m_writeCoilsCallback) {
-                            const uint8_t* values = &query[13];
+                            IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, coilValues.data());
-                            ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, values);
+                            exceptionCode = (uint8_t)result;
-                            if (result != ErrorCode::SUCCESS) {
-                                LOG_DEBUG("Write coils callback returned error: %d\n", (int)result);
-                            }
                         }
                     }
                 }
@@ -364,17 +372,15 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
 
         case MODBUS_FC_WRITE_SINGLE_REGISTER:
             {
-                if (queryLength >= 12) {
+                if (queryLength < 12) {
-                    uint16_t address = (query[8] << 8) | query[9];
+                    exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                    uint16_t value = (query[10] << 8) | query[11];
+                } else {
-                    
+                    address = (query[8] << 8) | query[9];
-                    LOG_DEBUG("Write single register - Address:%d, Value:%d\n", address, value);
+                    value = (query[10] << 8) | query[11];
                     
                     if (m_writeRegistersCallback) {
-                        ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value);
+                        IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value);
-                        if (result != ErrorCode::SUCCESS) {
+                        exceptionCode = (uint8_t)result;
-                            LOG_DEBUG("Write register callback returned error: %d\n", (int)result);
-                        }
                     }
                 }
             }
@@ -382,29 +388,25 @@ void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> cli
             
         case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
             {
-                if (queryLength >= 13) {
+                if (queryLength < 13) {
-                    uint16_t address = (query[8] << 8) | query[9];
+                    exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                    uint16_t quantity = (query[10] << 8) | query[11];
+                } else {
-                    uint8_t byteCount = query[12];
+                    address = (query[8] << 8) | query[9];
+                    quantity = (query[10] << 8) | query[11];
+                    byteCount = query[12];
                     
-                    if (queryLength >= (13 + byteCount) && byteCount == (quantity * 2)) {
+                    if (queryLength < (13 + byteCount) || byteCount != (quantity * 2)) {
-                        LOG_DEBUG("Write multiple registers - Address:%d, Quantity:%d, Bytes:%d\n", 
+                        exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
-                                  address, quantity, byteCount);
+                    } 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) {
-                            // 解析寄存器值（注意字节序）
+                            IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, registerValues.data());
-                            std::vector<uint16_t> values;
+                            exceptionCode = (uint8_t)result;
-                            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);
-                            }
                         }
                     }
                 }
@@ -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)