From f739b687d8249ba09528db644e376791ce95797e Mon Sep 17 00:00:00 2001 From: Dima Dorezyuk Date: Mon, 20 Nov 2023 10:55:51 +0100 Subject: [PATCH] SerialCommHub: support longer transactions, chunking and single register writes * make the timeout configurable * split long reads into chunks, to not overcome teh maximum ModBus message size. * Add single write command Co-authored-by: Dima Dorzyuk Signed-off-by: Dima Dorezyuk Signed-off-by: Evgeny Petrov --- interfaces/serial_communication_hub.yaml | 24 +++ modules/SerialCommHub/SerialCommHub.hpp | 2 +- .../main/serial_communication_hubImpl.cpp | 42 +++- .../main/serial_communication_hubImpl.hpp | 6 +- modules/SerialCommHub/manifest.yaml | 18 ++ modules/SerialCommHub/tiny_modbus_rtu.cpp | 188 +++++++++++++----- modules/SerialCommHub/tiny_modbus_rtu.hpp | 18 +- 7 files changed, 235 insertions(+), 63 deletions(-) diff --git a/interfaces/serial_communication_hub.yaml b/interfaces/serial_communication_hub.yaml index cd3117cd2d..075431ce7d 100644 --- a/interfaces/serial_communication_hub.yaml +++ b/interfaces/serial_communication_hub.yaml @@ -72,6 +72,30 @@ cmds: description: Status code of the transfer type: string $ref: /serial_comm_hub_requests#/StatusCodeEnum + modbus_write_single_register: + description: >- + Send a Modbus RTU 'write single register' 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 + register_address: + description: Address of the register to write to (16 bit address) + type: integer + minimum: 0 + maximum: 65535 + data: + description: Data content to be written to the above selected register + type: integer + minimum: 0 + maximum: 65535 + 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/SerialCommHub/SerialCommHub.hpp b/modules/SerialCommHub/SerialCommHub.hpp index 529523f671..b8b8c63e25 100644 --- a/modules/SerialCommHub/SerialCommHub.hpp +++ b/modules/SerialCommHub/SerialCommHub.hpp @@ -27,8 +27,8 @@ class SerialCommHub : public Everest::ModuleBase { SerialCommHub(const ModuleInfo& info, std::unique_ptr p_main, Conf& config) : ModuleBase(info), p_main(std::move(p_main)), config(config){}; - const Conf& config; const std::unique_ptr p_main; + const Conf& config; // ev@1fce4c5e-0ab8-41bb-90f7-14277703d2ac:v1 // insert your public definitions here diff --git a/modules/SerialCommHub/main/serial_communication_hubImpl.cpp b/modules/SerialCommHub/main/serial_communication_hubImpl.cpp index af6135e6cd..8302d36297 100644 --- a/modules/SerialCommHub/main/serial_communication_hubImpl.cpp +++ b/modules/SerialCommHub/main/serial_communication_hubImpl.cpp @@ -31,6 +31,7 @@ static std::vector vector_to_int(const std::vector& response) { // Implementation void serial_communication_hubImpl::init() { + using namespace std::chrono; Everest::GpioSettings rxtx_gpio_settings; rxtx_gpio_settings.chip_name = config.rxtx_gpio_chip; @@ -38,7 +39,8 @@ void serial_communication_hubImpl::init() { rxtx_gpio_settings.inverted = config.rxtx_gpio_tx_high; if (!modbus.open_device(config.serial_port, config.baudrate, config.ignore_echo, rxtx_gpio_settings, - static_cast(config.parity))) { + static_cast(config.parity), milliseconds(config.initial_timeout_ms), + milliseconds(config.within_message_timeout_ms))) { EVLOG_AND_THROW(Everest::EverestConfigError(fmt::format("Cannot open serial port {}.", config.serial_port))); } } @@ -66,7 +68,7 @@ serial_communication_hubImpl::handle_modbus_read_holding_registers(int& target_d // (uint16_t)first_register_address, (uint16_t)num_registers_to_read); response = modbus.txrx(target_device_id, tiny_modbus::FunctionCode::READ_MULTIPLE_HOLDING_REGISTERS, - first_register_address, num_registers_to_read); + first_register_address, num_registers_to_read, config.max_packet_size); if (response.size() > 0) { break; } @@ -102,7 +104,7 @@ serial_communication_hubImpl::handle_modbus_read_input_registers(int& target_dev // (uint16_t)first_register_address, (uint16_t)num_registers_to_read); response = modbus.txrx(target_device_id, tiny_modbus::FunctionCode::READ_INPUT_REGISTERS, - first_register_address, num_registers_to_read); + first_register_address, num_registers_to_read, config.max_packet_size); if (response.size() > 0) { break; } @@ -140,7 +142,7 @@ types::serial_comm_hub_requests::StatusCodeEnum serial_communication_hubImpl::ha (uint16_t)data.size()); response = modbus.txrx(target_device_id, tiny_modbus::FunctionCode::WRITE_MULTIPLE_HOLDING_REGISTERS, - first_register_address, data.size(), true, data); + first_register_address, data.size(), config.max_packet_size, true, data); if (response.size() > 0) { break; } @@ -156,6 +158,38 @@ types::serial_comm_hub_requests::StatusCodeEnum serial_communication_hubImpl::ha } } +types::serial_comm_hub_requests::StatusCodeEnum +serial_communication_hubImpl::handle_modbus_write_single_register(int& target_device_id, int& register_address, + int& data) { + types::serial_comm_hub_requests::Result result; + std::vector response; + + { + std::scoped_lock lock(serial_mutex); + + uint8_t retry_counter{this->num_resends_on_error}; + while (retry_counter-- > 0) { + + EVLOG_debug << fmt::format("Try {} Call modbus_client->write_single_register(id {} addr {} data {})", + (int)retry_counter, (uint8_t)target_device_id, (uint16_t)register_address, + (uint16_t)data); + + response = modbus.txrx(target_device_id, tiny_modbus::FunctionCode::WRITE_SINGLE_HOLDING_REGISTER, + register_address, 1, config.max_packet_size, true, {static_cast(data)}); + if (response.size() > 0) { + break; + } + } + } + EVLOG_debug << fmt::format("Done writing"); + // process response + if (response.size() > 0) { + return types::serial_comm_hub_requests::StatusCodeEnum::Success; + } else { + return types::serial_comm_hub_requests::StatusCodeEnum::Error; + } +} + void serial_communication_hubImpl::handle_nonstd_write(int& target_device_id, int& first_register_address, int& num_registers_to_read) { } diff --git a/modules/SerialCommHub/main/serial_communication_hubImpl.hpp b/modules/SerialCommHub/main/serial_communication_hubImpl.hpp index 6f10b7e105..02232bffc9 100644 --- a/modules/SerialCommHub/main/serial_communication_hubImpl.hpp +++ b/modules/SerialCommHub/main/serial_communication_hubImpl.hpp @@ -28,11 +28,13 @@ struct Conf { std::string serial_port; int baudrate; int parity; - int rs485_direction_gpio; bool ignore_echo; std::string rxtx_gpio_chip; int rxtx_gpio_line; bool rxtx_gpio_tx_high; + int max_packet_size; + int initial_timeout_ms; + int within_message_timeout_ms; }; class serial_communication_hubImpl : public serial_communication_hubImplBase { @@ -57,6 +59,8 @@ class serial_communication_hubImpl : public serial_communication_hubImplBase { virtual types::serial_comm_hub_requests::StatusCodeEnum handle_modbus_write_multiple_registers(int& target_device_id, int& first_register_address, 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 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/SerialCommHub/manifest.yaml b/modules/SerialCommHub/manifest.yaml index b3fd10986d..ab187548a7 100644 --- a/modules/SerialCommHub/manifest.yaml +++ b/modules/SerialCommHub/manifest.yaml @@ -36,6 +36,24 @@ provides: description: GPIO direction, false means low for TX, true means high for TX type: boolean default: false + max_packet_size: + description: >- + Maximum size of a packet to read/write in bytes. Payload exceeding the size will be chunked. + The APU size according to [wikipedia](https://en.wikipedia.org/wiki/Modbus) is 256 bytes, + which is used as default here. + type: integer + # 7 is a minimum packet size to transfer a response + minimum: 7 + maximum: 65536 + default: 256 + initial_timeout_ms: + description: Timeout in ms for the first packet. + type: integer + default: 500 + within_message_timeout_ms: + description: Timeout in ms for subsequent packets. + type: integer + default: 100 metadata: license: https://opensource.org/licenses/Apache-2.0 authors: diff --git a/modules/SerialCommHub/tiny_modbus_rtu.cpp b/modules/SerialCommHub/tiny_modbus_rtu.cpp index 3a5cfe2fdb..4ee7f0e48e 100644 --- a/modules/SerialCommHub/tiny_modbus_rtu.cpp +++ b/modules/SerialCommHub/tiny_modbus_rtu.cpp @@ -21,7 +21,10 @@ #include #include +#include #include +#include +#include #include "crc16.hpp" @@ -174,6 +177,8 @@ static std::vector decode_reply(const uint8_t* buf, int len, uint8_t e case 0x0B: EVLOG_error << "Modbus exception: Gateway target device failed to respond"; break; + default: + EVLOG_error << "Modbus exception: Unknown"; } return result; } @@ -185,8 +190,12 @@ TinyModbusRTU::~TinyModbusRTU() { } bool TinyModbusRTU::open_device(const std::string& device, int _baud, bool _ignore_echo, - const Everest::GpioSettings& rxtx_gpio_settings, const Parity parity) { + const Everest::GpioSettings& rxtx_gpio_settings, const Parity parity, + std::chrono::milliseconds _initial_timeout, + std::chrono::milliseconds _within_message_timeout) { + initial_timeout = _initial_timeout; + within_message_timeout = _within_message_timeout; ignore_echo = _ignore_echo; rxtx_gpio.open(rxtx_gpio_settings); @@ -235,14 +244,14 @@ bool TinyModbusRTU::open_device(const std::string& device, int _baud, bool _igno // disable IGNBRK for mismatched speed tests; otherwise receive break // as \000 chars tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY); - tty.c_lflag = 0; // no signaling chars, no echo, - // no canonical processing - tty.c_oflag = 0; // no remapping, no delays - tty.c_cc[VMIN] = 1; // read blocks - tty.c_cc[VTIME] = 1; // 0.1 seconds inter character read timeout after first byte was received - - tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, - // enable reading + tty.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 1; // read blocks + tty.c_cc[VTIME] = 1; // 0.1 seconds inter character read timeout after first byte was received + + tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, + // enable reading if (parity == Parity::ODD) { tty.c_cflag |= (PARENB | PARODD); // odd parity } else if (parity == Parity::EVEN) { // even parity @@ -251,7 +260,7 @@ bool TinyModbusRTU::open_device(const std::string& device, int _baud, bool _igno } else { tty.c_cflag &= ~(PARENB | PARODD); // shut off parity } - tty.c_cflag &= ~CSTOPB; // 1 Stop bit + tty.c_cflag &= ~CSTOPB; // 1 Stop bit tty.c_cflag &= ~CRTSCTS; if (tcsetattr(fd, TCSANOW, &tty) != 0) { @@ -262,9 +271,19 @@ bool TinyModbusRTU::open_device(const std::string& device, int _baud, bool _igno } int TinyModbusRTU::read_reply(uint8_t* rxbuf, int rxbuf_len) { - struct timeval timeout; - timeout.tv_sec = 0; - timeout.tv_usec = MODBUS_RX_INITIAL_TIMEOUT_MS * 1000; // 500msec intial timeout until device responds + // Lambda to convert std::chrono to timeval. + auto to_timeval = [](const auto& time) { + using namespace std::chrono; + struct timeval timeout; + auto sec = duration_cast(time); + timeout.tv_sec = sec.count(); + timeout.tv_usec = duration_cast(time - sec).count(); + return timeout; + }; + + auto timeout = to_timeval(initial_timeout); + const auto within_message_timeval = to_timeval(within_message_timeout); + fd_set set; FD_ZERO(&set); FD_SET(fd, &set); @@ -272,14 +291,13 @@ int TinyModbusRTU::read_reply(uint8_t* rxbuf, int rxbuf_len) { int bytes_read_total = 0; while (true) { int rv = select(fd + 1, &set, NULL, NULL, &timeout); - timeout.tv_usec = MODBUS_RX_WITHIN_MESSAGE_TIMEOUT_MS * - 1000; // reduce timeout after first chunk, no uneccesary waiting at the end of the message - if (rv == -1) { // error in select function call + timeout = within_message_timeval; + if (rv == -1) { // error in select function call perror("txrx: select:"); break; } else if (rv == 0) { // no more bytes to read within timeout, so transfer is complete break; - } else { // received more bytes, add them to buffer + } else { // received more bytes, add them to buffer // do we have space in the rx buffer left? if (bytes_read_total >= rxbuf_len) { // no buffer space left, but more to read. @@ -289,56 +307,122 @@ int TinyModbusRTU::read_reply(uint8_t* rxbuf, int rxbuf_len) { int bytes_read = read(fd, rxbuf + bytes_read_total, rxbuf_len - bytes_read_total); if (bytes_read > 0) { bytes_read_total += bytes_read; - // EVLOG_info << "RECVD: " << hexdump(rxbuf, bytes_read_total); } } } return bytes_read_total; } -/* - This function transmits a modbus request and waits for the reply. - Parameter request is optional and is only used for writing multiple registers. -*/ std::vector TinyModbusRTU::txrx(uint8_t device_address, FunctionCode function, uint16_t first_register_address, uint16_t register_quantity, - bool wait_for_reply, std::vector request) { - { - // size of request - int req_len = (request.size() == 0 ? 0 : 2 * request.size() + 1) + MODBUS_BASE_PAYLOAD_SIZE; - std::unique_ptr req(new uint8_t[req_len]); - - // add header - req[DEVICE_ADDRESS_POS] = device_address; - req[FUNCTION_CODE_POS] = function; - - first_register_address = htobe16(first_register_address); - register_quantity = htobe16(register_quantity); - memcpy(req.get() + REQ_TX_FIRST_REGISTER_ADDR_POS, &first_register_address, 2); - memcpy(req.get() + REQ_TX_QUANTITY_POS, ®ister_quantity, 2); - - if (function == FunctionCode::WRITE_MULTIPLE_HOLDING_REGISTERS) { - // write byte count - req[REQ_TX_MULTIPLE_REG_BYTE_COUNT_POS] = request.size() * 2; - // add request data - int i = REQ_TX_MULTIPLE_REG_BYTE_COUNT_POS + 1; - for (auto r : request) { - r = htobe16(r); - memcpy(req.get() + i, &r, 2); - i += 2; - } + uint16_t max_packet_size, bool wait_for_reply, + std::vector request) { + // This only supports chunking of the read-requests. + std::vector out; + + if (max_packet_size < MODBUS_MIN_REPLY_SIZE + 2) { + EVLOG_error << fmt::format("Max packet size too small: {}", max_packet_size); + return {}; + } + + const uint16_t register_chunk = (max_packet_size - MODBUS_MIN_REPLY_SIZE) / 2; + size_t written_elements = 0; + while (register_quantity) { + const auto current_register_quantity = std::min(register_quantity, register_chunk); + std::vector current_request; + if (request.size() > written_elements + current_register_quantity) { + current_request = std::vector(request.begin() + written_elements, + request.begin() + written_elements + current_register_quantity); + written_elements += current_register_quantity; + } else { + current_request = std::vector(request.begin() + written_elements, request.end()); + written_elements = request.size(); + } + + const auto res = txrx_impl(device_address, function, first_register_address, current_register_quantity, + wait_for_reply, current_request); + + // We failed to read/write. + if (res.empty()) { + return res; } - // set checksum in the last 2 bytes - append_checksum(req.get(), req_len); - // EVLOG_info << "SEND: " << hexdump(req.get(), req_len); + out.insert(out.end(), res.begin(), res.end()); + first_register_address += current_register_quantity; + register_quantity -= current_register_quantity; + } + + return out; +} +std::vector _make_single_write_request(uint8_t device_address, 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); + + register_address = htobe16(register_address); + data = htobe16(data); + memcpy(req.data() + REQ_TX_FIRST_REGISTER_ADDR_POS, ®ister_address, 2); + memcpy(req.data() + REQ_TX_SINGLE_REG_PAYLOAD_POS, &data, 2); + append_checksum(req.data(), req_len); + + return req; +} + +std::vector _make_generic_request(uint8_t device_address, FunctionCode function, + uint16_t first_register_address, uint16_t register_quantity, + std::vector request) { + // size of request + int req_len = (request.size() == 0 ? 0 : 2 * request.size() + 1) + MODBUS_BASE_PAYLOAD_SIZE; + std::vector req(req_len); + + // add header + req[DEVICE_ADDRESS_POS] = device_address; + req[FUNCTION_CODE_POS] = function; + + first_register_address = htobe16(first_register_address); + register_quantity = htobe16(register_quantity); + memcpy(req.data() + REQ_TX_FIRST_REGISTER_ADDR_POS, &first_register_address, 2); + memcpy(req.data() + REQ_TX_QUANTITY_POS, ®ister_quantity, 2); + + if (function == FunctionCode::WRITE_MULTIPLE_HOLDING_REGISTERS) { + // write byte count + req[REQ_TX_MULTIPLE_REG_BYTE_COUNT_POS] = request.size() * 2; + // add request data + int i = REQ_TX_MULTIPLE_REG_BYTE_COUNT_POS + 1; + for (auto r : request) { + r = htobe16(r); + memcpy(req.data() + i, &r, 2); + i += 2; + } + } + + // set checksum in the last 2 bytes + append_checksum(req.data(), req_len); + + return req; +} +/* + This function transmits a modbus request and waits for the reply. + Parameter request is optional and is only used for writing multiple registers. +*/ +std::vector TinyModbusRTU::txrx_impl(uint8_t device_address, FunctionCode function, + uint16_t first_register_address, uint16_t register_quantity, + bool wait_for_reply, std::vector request) { + { + auto req = + function == FunctionCode::WRITE_SINGLE_HOLDING_REGISTER + ? _make_single_write_request(device_address, 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); // write to serial port rxtx_gpio.set(false); - write(fd, req.get(), req_len); + write(fd, req.data(), req.size()); if (rxtx_gpio.is_ready()) { // if we are using GPIO to switch between RX/TX, use the fast version of tcdrain with exact timing fast_tcdrain(fd); @@ -350,7 +434,7 @@ std::vector TinyModbusRTU::txrx(uint8_t device_address, FunctionCode f if (ignore_echo) { // read back echo of what we sent and ignore it - read_reply(req.get(), req_len); + read_reply(req.data(), req.size()); } } diff --git a/modules/SerialCommHub/tiny_modbus_rtu.hpp b/modules/SerialCommHub/tiny_modbus_rtu.hpp index 669c9cb049..f843f8b8cf 100644 --- a/modules/SerialCommHub/tiny_modbus_rtu.hpp +++ b/modules/SerialCommHub/tiny_modbus_rtu.hpp @@ -7,6 +7,7 @@ #ifndef TINY_MODBUS_RTU #define TINY_MODBUS_RTU +#include #include #include @@ -20,6 +21,7 @@ constexpr int FUNCTION_CODE_POS = 0x01; constexpr int REQ_TX_FIRST_REGISTER_ADDR_POS = 0x02; constexpr int REQ_TX_QUANTITY_POS = 0x04; +constexpr int REQ_TX_SINGLE_REG_PAYLOAD_POS = 0x04; constexpr int REQ_TX_MULTIPLE_REG_BYTE_COUNT_POS = 0x06; @@ -31,9 +33,6 @@ constexpr int MODBUS_MAX_REPLY_SIZE = 255 + 6; constexpr int MODBUS_MIN_REPLY_SIZE = 5; constexpr int MODBUS_BASE_PAYLOAD_SIZE = 8; -constexpr int MODBUS_RX_INITIAL_TIMEOUT_MS = 500; -constexpr int MODBUS_RX_WITHIN_MESSAGE_TIMEOUT_MS = 100; - enum class Parity : uint8_t { NONE = 0, ODD = 1, @@ -57,18 +56,27 @@ class TinyModbusRTU { ~TinyModbusRTU(); bool open_device(const std::string& device, int baud, bool ignore_echo, - const Everest::GpioSettings& rxtx_gpio_settings, const Parity parity); + const Everest::GpioSettings& rxtx_gpio_settings, const Parity parity, + std::chrono::milliseconds initial_timeout, std::chrono::milliseconds within_message_timeout); + std::vector txrx(uint8_t device_address, FunctionCode function, uint16_t first_register_address, - uint16_t register_quantity, bool wait_for_reply = true, + uint16_t register_quantity, uint16_t chunk_size, bool wait_for_reply = true, std::vector request = std::vector()); private: // Serial interface int fd{0}; bool ignore_echo{false}; + + std::vector txrx_impl(uint8_t device_address, FunctionCode function, uint16_t first_register_address, + uint16_t register_quantity, bool wait_for_reply = true, + std::vector request = std::vector()); + int read_reply(uint8_t* rxbuf, int rxbuf_len); Everest::Gpio rxtx_gpio; + std::chrono::milliseconds initial_timeout; + std::chrono::milliseconds within_message_timeout; }; } // namespace tiny_modbus