From 2a81333224a11f6c4f6b64d4997b2c2e1a3c616b Mon Sep 17 00:00:00 2001 From: Mark Oude Elberink Date: Wed, 10 Sep 2025 16:22:27 +0200 Subject: [PATCH] Add interface cmds for read/write coils * Fix Modbus RTU Single Coil implementation * Implement handler functions for both new commands Signed-off-by: Mark Oude Elberink --- interfaces/serial_communication_hub.yaml | 46 ++++++++++++++ .../main/serial_communication_hubImpl.cpp | 60 +++++++++++++++++++ .../main/serial_communication_hubImpl.hpp | 4 ++ .../Misc/SerialCommHub/tiny_modbus_rtu.cpp | 11 ++-- types/serial_comm_hub_requests.yaml | 13 ++++ 5 files changed, 129 insertions(+), 5 deletions(-) diff --git a/interfaces/serial_communication_hub.yaml b/interfaces/serial_communication_hub.yaml index 075431ce7d..c88a1f959d 100644 --- a/interfaces/serial_communication_hub.yaml +++ b/interfaces/serial_communication_hub.yaml @@ -96,6 +96,52 @@ cmds: description: Status code of the transfer type: string $ref: /serial_comm_hub_requests#/StatusCodeEnum + modbus_read_coils: + description: >- + Send a Modbus RTU 'read coils' command via serial interface to the target + hardware. (return value: response) + arguments: + target_device_id: + description: ID (1 byte) of the device to send the commands to + type: integer + minimum: 0 + maximum: 255 + first_coil_address: + description: Start address for read operation (16 bit address) + type: integer + minimum: 0 + maximum: 65535 + num_coils_to_read: + description: Number of coils to read (1 bit each) + type: integer + minimum: 1 + maximum: 65535 + result: + description: Result of the transfer + type: object + $ref: /serial_comm_hub_requests#/ResultBool + modbus_write_single_coil: + description: >- + Send a Modbus RTU 'write single coil' command via serial interface to + the target hardware. (return value: response) + arguments: + target_device_id: + description: ID (1 byte) of the device to send the commands to + type: integer + minimum: 0 + maximum: 255 + coil_address: + description: Address of the coil to write to (16 bit address) + type: integer + minimum: 0 + maximum: 65535 + data: + description: Data content to be written to the selected coil + type: boolean + result: + description: Status code of the transfer + type: string + $ref: /serial_comm_hub_requests#/StatusCodeEnum nonstd_write: description: >- Non standard mode to write registers in read discrete input mode diff --git a/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.cpp b/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.cpp index af5175d094..f035fd9591 100644 --- a/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.cpp +++ b/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.cpp @@ -30,6 +30,47 @@ static std::vector vector_to_int(const std::vector& response) { return i; } +/** + * @brief Converts a Result to a ResultBool by looking at each bit of the uint16_t values and converting them to + * bools in the right order. Used for Modbus read coils responses where the result is a bit-packed array of coil states. + * @param result The Result to convert + * @param number_of_coils The number of coils that were requested to read, used to limit the number of bools in the + * output + * @return The converted ResultBool + */ +static types::serial_comm_hub_requests::ResultBool +convert_read_coils_result(const types::serial_comm_hub_requests::Result& result, size_t number_of_coils) { + constexpr uint8_t BITS_PER_BYTE = 8; + constexpr uint16_t BYTE_MASK = 0xFF; + + types::serial_comm_hub_requests::ResultBool out; + out.status_code = result.status_code; + + if (result.value.has_value()) { + std::vector result_bool; + for (const uint16_t packed_bytes : result.value.value()) { + // Modbus read coils response packs bits into raw bytes, the modbus library uses big-endian to build uint16 + // from those. Here we extract the original MSB and LSB from the BE uint16_t and process them in the correct + // order. + const auto msb = static_cast((packed_bytes >> BITS_PER_BYTE) & BYTE_MASK); + const auto lsb = static_cast(packed_bytes & BYTE_MASK); + + for (const uint8_t byte : {msb, lsb}) { + for (int bit = 0; bit < BITS_PER_BYTE; bit++) { + if (result_bool.size() >= number_of_coils) { + break; + } + result_bool.push_back((byte & (1U << bit)) != 0); + } + } + } + + out.value = std::move(result_bool); + } + + return out; +} + // Implementation void serial_communication_hubImpl::init() { @@ -150,6 +191,25 @@ serial_communication_hubImpl::handle_modbus_write_single_register(int& target_de return result.status_code; } +types::serial_comm_hub_requests::StatusCodeEnum +serial_communication_hubImpl::handle_modbus_write_single_coil(int& target_device_id, int& coil_address, bool& data) { + types::serial_comm_hub_requests::Result result; + + result = perform_modbus_request(target_device_id, tiny_modbus::FunctionCode::WRITE_SINGLE_COIL, coil_address, 1, + true, {static_cast(data ? 0xFF00 : 0x0000)}); + + return result.status_code; +} + +types::serial_comm_hub_requests::ResultBool +serial_communication_hubImpl::handle_modbus_read_coils(int& target_device_id, int& first_coil_address, + int& num_coils_to_read) { + const auto result = perform_modbus_request(target_device_id, tiny_modbus::FunctionCode::READ_COILS, + first_coil_address, num_coils_to_read); + + return convert_read_coils_result(result, num_coils_to_read); +} + void serial_communication_hubImpl::handle_nonstd_write(int& target_device_id, int& first_register_address, int& num_registers_to_read) { } diff --git a/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.hpp b/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.hpp index 41b7c98cf1..9ad57a957f 100644 --- a/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.hpp +++ b/modules/Misc/SerialCommHub/main/serial_communication_hubImpl.hpp @@ -64,6 +64,10 @@ class serial_communication_hubImpl : public serial_communication_hubImplBase { types::serial_comm_hub_requests::VectorUint16& data_raw) override; virtual types::serial_comm_hub_requests::StatusCodeEnum handle_modbus_write_single_register(int& target_device_id, int& register_address, int& data) override; + virtual types::serial_comm_hub_requests::ResultBool + handle_modbus_read_coils(int& target_device_id, int& first_coil_address, int& num_coils_to_read) override; + virtual types::serial_comm_hub_requests::StatusCodeEnum + handle_modbus_write_single_coil(int& target_device_id, int& coil_address, bool& data) override; virtual void handle_nonstd_write(int& target_device_id, int& first_register_address, int& num_registers_to_read) override; virtual types::serial_comm_hub_requests::Result diff --git a/modules/Misc/SerialCommHub/tiny_modbus_rtu.cpp b/modules/Misc/SerialCommHub/tiny_modbus_rtu.cpp index dba1693055..7486ecee10 100644 --- a/modules/Misc/SerialCommHub/tiny_modbus_rtu.cpp +++ b/modules/Misc/SerialCommHub/tiny_modbus_rtu.cpp @@ -416,13 +416,13 @@ std::vector TinyModbusRTU::txrx(uint8_t device_address, FunctionCode f return out; } -std::vector _make_single_write_request(uint8_t device_address, uint16_t register_address, bool wait_for_reply, - uint16_t data) { +std::vector _make_single_write_request(uint8_t device_address, FunctionCode function, + uint16_t register_address, bool wait_for_reply, uint16_t data) { const int req_len = 8; std::vector req(req_len); req[DEVICE_ADDRESS_POS] = device_address; - req[FUNCTION_CODE_POS] = static_cast(FunctionCode::WRITE_SINGLE_HOLDING_REGISTER); + req[FUNCTION_CODE_POS] = static_cast(function); register_address = htobe16(register_address); data = htobe16(data); @@ -479,8 +479,9 @@ std::vector TinyModbusRTU::txrx_impl(uint8_t device_address, FunctionC } auto req = - function == FunctionCode::WRITE_SINGLE_HOLDING_REGISTER - ? _make_single_write_request(device_address, first_register_address, wait_for_reply, request.at(0)) + function == FunctionCode::WRITE_SINGLE_HOLDING_REGISTER or function == FunctionCode::WRITE_SINGLE_COIL + ? _make_single_write_request(device_address, function, first_register_address, wait_for_reply, + request.at(0)) : _make_generic_request(device_address, function, first_register_address, register_quantity, request); // clear input and output buffer tcflush(fd, TCIOFLUSH); diff --git a/types/serial_comm_hub_requests.yaml b/types/serial_comm_hub_requests.yaml index 8a2eb067c9..71d5028909 100644 --- a/types/serial_comm_hub_requests.yaml +++ b/types/serial_comm_hub_requests.yaml @@ -21,6 +21,19 @@ types: type: integer minimum: 0 maximum: 65535 + ResultBool: + description: Return type for IO transfer functions with boolean return values, e.g. for coil states + type: object + required: + - status_code + properties: + status_code: + type: string + $ref: /serial_comm_hub_requests#/StatusCodeEnum + value: + type: array + items: + type: boolean VectorUint16: description: Data content (raw data bytes) type: object