fix modbus server response

markaren · Sep 27, 2024 · ee3715a · ee3715a
1 parent 2b6f68c
commit ee3715a
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 8 deletions.
diff --git a/src/simple_socket/modbus/ModbusServer.cpp b/src/simple_socket/modbus/ModbusServer.cpp
@@ -35,15 +35,21 @@ namespace {
                     return;
                 }
 
-                // Prepare response: device address, function code, byte count
-                std::vector response(request.begin(), request.begin() + 8);
-                response.push_back(quantity * 2);// Byte count (2 bytes per register)
-
+                // Prepare response: MBAP header + PDU
+                std::vector response(request.begin(), request.begin()+4);
+                const uint16_t length = 3 + (quantity * 2); // Length of PDU
+                response.push_back(length >> 8); // Length (High)
+                response.push_back(length & 0xFF); // Length (Low)
+                response.push_back(request[6]); // Unit Identifier
+
+                // PDU
+                response.push_back(functionCode); // Function Code
+                response.push_back(quantity * 2);  // Byte Count
                 // Append register values to the response
                 for (uint16_t i = 0; i < quantity; ++i) {
                     const uint16_t regValue = reg.getUint16(startAddress + i);
                     response.push_back(regValue >> 8);  // High byte
-                    response.push_back(regValue & 0xFF);// Low byte
+                    response.push_back(regValue & 0xFF); // Low byte
                 }
 
                 // Send the response

diff --git a/tests/integration/modbus_server.cpp b/tests/integration/modbus_server.cpp
@@ -16,7 +16,20 @@ int main() {
     ModbusServer server(holding_register, 502);
     server.start();
 
-    do {
-        std::cout << "Press a key to continue..." << std::endl;
-    } while (std::cin.get() != '\n');
+    std::atomic_bool stop{false};
+    std::thread t([&] {
+        while (!stop) {
+            holding_register.setUint16(0, holding_register.getUint16(0)+1);
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
+    });
+
+
+    std::cout << "Press a key to continue..." << std::endl;
+    std::cin.get();
+
+    server.stop();
+    stop = true;
+
+    if (t.joinable()) t.join();
 }