From 1ca7c2d7ddc44f8debe288a98fbca3e40796f5cf Mon Sep 17 00:00:00 2001 From: Jeroen van Oort Date: Wed, 22 May 2024 06:17:32 +0200 Subject: [PATCH] Add support for acting as Modbus server (#4874) Co-authored-by: Jeroen van Oort Co-authored-by: Jesse Hills <3060199+jesserockz@users.noreply.github.com> --- esphome/components/modbus/__init__.py | 35 ++++++++++++ esphome/components/modbus/modbus.cpp | 27 ++++++--- esphome/components/modbus/modbus.h | 9 +++ .../components/modbus_controller/__init__.py | 52 +++++++++++++++-- .../modbus_controller/modbus_controller.cpp | 57 +++++++++++++++++-- .../modbus_controller/modbus_controller.h | 22 +++++++ .../modbus_controller/test.esp32.yaml | 22 ++++++- 7 files changed, 203 insertions(+), 21 deletions(-) diff --git a/esphome/components/modbus/__init__.py b/esphome/components/modbus/__init__.py index 6fea7033f2..ae0c818c28 100644 --- a/esphome/components/modbus/__init__.py +++ b/esphome/components/modbus/__init__.py @@ -1,5 +1,9 @@ +from __future__ import annotations +from typing import Literal + import esphome.codegen as cg import esphome.config_validation as cv +import esphome.final_validate as fv from esphome.cpp_helpers import gpio_pin_expression from esphome.components import uart from esphome.const import ( @@ -17,13 +21,21 @@ Modbus = modbus_ns.class_("Modbus", cg.Component, uart.UARTDevice) ModbusDevice = modbus_ns.class_("ModbusDevice") MULTI_CONF = True +CONF_ROLE = "role" CONF_MODBUS_ID = "modbus_id" CONF_SEND_WAIT_TIME = "send_wait_time" +ModbusRole = modbus_ns.enum("ModbusRole") +MODBUS_ROLES = { + "client": ModbusRole.CLIENT, + "server": ModbusRole.SERVER, +} + CONFIG_SCHEMA = ( cv.Schema( { cv.GenerateID(): cv.declare_id(Modbus), + cv.Optional(CONF_ROLE, default="client"): cv.enum(MODBUS_ROLES), cv.Optional(CONF_FLOW_CONTROL_PIN): pins.gpio_output_pin_schema, cv.Optional( CONF_SEND_WAIT_TIME, default="250ms" @@ -43,6 +55,7 @@ async def to_code(config): await uart.register_uart_device(var, config) + cg.add(var.set_role(config[CONF_ROLE])) if CONF_FLOW_CONTROL_PIN in config: pin = await gpio_pin_expression(config[CONF_FLOW_CONTROL_PIN]) cg.add(var.set_flow_control_pin(pin)) @@ -62,6 +75,28 @@ def modbus_device_schema(default_address): return cv.Schema(schema) +def final_validate_modbus_device( + name: str, *, role: Literal["server", "client"] | None = None +): + def validate_role(value): + assert role in MODBUS_ROLES + if value != role: + raise cv.Invalid(f"Component {name} requires role to be {role}") + return value + + def validate_hub(hub_config): + hub_schema = {} + if role is not None: + hub_schema[cv.Required(CONF_ROLE)] = validate_role + + return cv.Schema(hub_schema, extra=cv.ALLOW_EXTRA)(hub_config) + + return cv.Schema( + {cv.Required(CONF_MODBUS_ID): fv.id_declaration_match_schema(validate_hub)}, + extra=cv.ALLOW_EXTRA, + ) + + async def register_modbus_device(var, config): parent = await cg.get_variable(config[CONF_MODBUS_ID]) cg.add(var.set_parent(parent)) diff --git a/esphome/components/modbus/modbus.cpp b/esphome/components/modbus/modbus.cpp index 137fb0b26b..f8dd4c18b9 100644 --- a/esphome/components/modbus/modbus.cpp +++ b/esphome/components/modbus/modbus.cpp @@ -77,7 +77,13 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) { ESP_LOGD(TAG, "Modbus user-defined function %02X found", function_code); } else { - // the response for write command mirrors the requests and data startes at offset 2 instead of 3 for read commands + // data starts at 2 and length is 4 for read registers commands + if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) { + data_offset = 2; + data_len = 4; + } + + // the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) { data_offset = 2; data_len = 4; @@ -123,6 +129,9 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) { // Ignore modbus exception not related to a pending command ESP_LOGD(TAG, "Ignoring Modbus error - not expecting a response"); } + } else if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) { + device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8), + uint16_t(data[3]) | (uint16_t(data[2]) << 8)); } else { device->on_modbus_data(data); } @@ -164,16 +173,18 @@ void Modbus::send(uint8_t address, uint8_t function_code, uint16_t start_address std::vector data; data.push_back(address); data.push_back(function_code); - data.push_back(start_address >> 8); - data.push_back(start_address >> 0); - if (function_code != 0x5 && function_code != 0x6) { - data.push_back(number_of_entities >> 8); - data.push_back(number_of_entities >> 0); + if (this->role == ModbusRole::CLIENT) { + data.push_back(start_address >> 8); + data.push_back(start_address >> 0); + if (function_code != 0x5 && function_code != 0x6) { + data.push_back(number_of_entities >> 8); + data.push_back(number_of_entities >> 0); + } } if (payload != nullptr) { - if (function_code == 0xF || function_code == 0x10) { // Write multiple - data.push_back(payload_len); // Byte count is required for write + if (this->role == ModbusRole::SERVER || function_code == 0xF || function_code == 0x10) { // Write multiple + data.push_back(payload_len); // Byte count is required for write } else { payload_len = 2; // Write single register or coil } diff --git a/esphome/components/modbus/modbus.h b/esphome/components/modbus/modbus.h index dd8732c6e9..4a78ed4aab 100644 --- a/esphome/components/modbus/modbus.h +++ b/esphome/components/modbus/modbus.h @@ -8,6 +8,11 @@ namespace esphome { namespace modbus { +enum ModbusRole { + CLIENT, + SERVER, +}; + class ModbusDevice; class Modbus : public uart::UARTDevice, public Component { @@ -27,11 +32,14 @@ class Modbus : public uart::UARTDevice, public Component { void send(uint8_t address, uint8_t function_code, uint16_t start_address, uint16_t number_of_entities, uint8_t payload_len = 0, const uint8_t *payload = nullptr); void send_raw(const std::vector &payload); + void set_role(ModbusRole role) { this->role = role; } void set_flow_control_pin(GPIOPin *flow_control_pin) { this->flow_control_pin_ = flow_control_pin; } uint8_t waiting_for_response{0}; void set_send_wait_time(uint16_t time_in_ms) { send_wait_time_ = time_in_ms; } void set_disable_crc(bool disable_crc) { disable_crc_ = disable_crc; } + ModbusRole role; + protected: GPIOPin *flow_control_pin_{nullptr}; @@ -50,6 +58,7 @@ class ModbusDevice { void set_address(uint8_t address) { address_ = address; } virtual void on_modbus_data(const std::vector &data) = 0; virtual void on_modbus_error(uint8_t function_code, uint8_t exception_code) {} + virtual void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers){}; void send(uint8_t function, uint16_t start_address, uint16_t number_of_entities, uint8_t payload_len = 0, const uint8_t *payload = nullptr) { this->parent_->send(this->address_, function, start_address, number_of_entities, payload_len, payload); diff --git a/esphome/components/modbus_controller/__init__.py b/esphome/components/modbus_controller/__init__.py index 8703771c3a..b8ab48fcc6 100644 --- a/esphome/components/modbus_controller/__init__.py +++ b/esphome/components/modbus_controller/__init__.py @@ -23,6 +23,8 @@ CODEOWNERS = ["@martgras"] AUTO_LOAD = ["modbus"] +CONF_READ_LAMBDA = "read_lambda" +CONF_SERVER_REGISTERS = "server_registers" MULTI_CONF = True modbus_controller_ns = cg.esphome_ns.namespace("modbus_controller") @@ -31,6 +33,7 @@ ModbusController = modbus_controller_ns.class_( ) SensorItem = modbus_controller_ns.struct("SensorItem") +ServerRegister = modbus_controller_ns.struct("ServerRegister") ModbusFunctionCode_ns = modbus_controller_ns.namespace("ModbusFunctionCode") ModbusFunctionCode = ModbusFunctionCode_ns.enum("ModbusFunctionCode") @@ -94,10 +97,18 @@ TYPE_REGISTER_MAP = { "FP32_R": 2, } -MULTI_CONF = True - _LOGGER = logging.getLogger(__name__) +ModbusServerRegisterSchema = cv.Schema( + { + cv.GenerateID(): cv.declare_id(ServerRegister), + cv.Required(CONF_ADDRESS): cv.positive_int, + cv.Optional(CONF_VALUE_TYPE, default="U_WORD"): cv.enum(SENSOR_VALUE_TYPE), + cv.Required(CONF_READ_LAMBDA): cv.returning_lambda, + } +) + + CONFIG_SCHEMA = cv.All( cv.Schema( { @@ -106,6 +117,9 @@ CONFIG_SCHEMA = cv.All( CONF_COMMAND_THROTTLE, default="0ms" ): cv.positive_time_period_milliseconds, cv.Optional(CONF_OFFLINE_SKIP_UPDATES, default=0): cv.positive_int, + cv.Optional( + CONF_SERVER_REGISTERS, + ): cv.ensure_list(ModbusServerRegisterSchema), } ) .extend(cv.polling_component_schema("60s")) @@ -154,6 +168,17 @@ def validate_modbus_register(config): return config +def _final_validate(config): + if CONF_SERVER_REGISTERS in config: + return modbus.final_validate_modbus_device("modbus_controller", role="server")( + config + ) + return config + + +FINAL_VALIDATE_SCHEMA = _final_validate + + def modbus_calc_properties(config): byte_offset = 0 reg_count = 0 @@ -183,7 +208,7 @@ def modbus_calc_properties(config): async def add_modbus_base_properties( - var, config, sensor_type, lamdba_param_type=cg.float_, lamdba_return_type=float + var, config, sensor_type, lambda_param_type=cg.float_, lambda_return_type=float ): if CONF_CUSTOM_COMMAND in config: cg.add(var.set_custom_data(config[CONF_CUSTOM_COMMAND])) @@ -196,13 +221,13 @@ async def add_modbus_base_properties( config[CONF_LAMBDA], [ (sensor_type.operator("ptr"), "item"), - (lamdba_param_type, "x"), + (lambda_param_type, "x"), ( cg.std_vector.template(cg.uint8).operator("const").operator("ref"), "data", ), ], - return_type=cg.optional.template(lamdba_return_type), + return_type=cg.optional.template(lambda_return_type), ) cg.add(var.set_template(template_)) @@ -211,6 +236,23 @@ async def to_code(config): var = cg.new_Pvariable(config[CONF_ID]) cg.add(var.set_command_throttle(config[CONF_COMMAND_THROTTLE])) cg.add(var.set_offline_skip_updates(config[CONF_OFFLINE_SKIP_UPDATES])) + if CONF_SERVER_REGISTERS in config: + for server_register in config[CONF_SERVER_REGISTERS]: + cg.add( + var.add_server_register( + cg.new_Pvariable( + server_register[CONF_ID], + server_register[CONF_ADDRESS], + server_register[CONF_VALUE_TYPE], + TYPE_REGISTER_MAP[server_register[CONF_VALUE_TYPE]], + await cg.process_lambda( + server_register[CONF_READ_LAMBDA], + [], + return_type=cg.float_, + ), + ) + ) + ) await register_modbus_device(var, config) diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp index 7565dc5e1b..9f73988b03 100644 --- a/esphome/components/modbus_controller/modbus_controller.cpp +++ b/esphome/components/modbus_controller/modbus_controller.cpp @@ -7,10 +7,7 @@ namespace modbus_controller { static const char *const TAG = "modbus_controller"; -void ModbusController::setup() { - // Modbus::setup(); - this->create_register_ranges_(); -} +void ModbusController::setup() { this->create_register_ranges_(); } /* To work with the existing modbus class and avoid polling for responses a command queue is used. @@ -102,6 +99,51 @@ void ModbusController::on_modbus_error(uint8_t function_code, uint8_t exception_ } } +void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t start_address, + uint16_t number_of_registers) { + ESP_LOGD(TAG, + "Received read holding/input registers for device 0x%X. FC: 0x%X. Start address: 0x%X. Number of registers: " + "0x%X.", + this->address_, function_code, start_address, number_of_registers); + + std::vector sixteen_bit_response; + for (uint16_t current_address = start_address; current_address < start_address + number_of_registers;) { + bool found = false; + for (auto *server_register : this->server_registers_) { + if (server_register->address == current_address) { + float value = server_register->read_lambda(); + + ESP_LOGD(TAG, "Matched register. Address: 0x%02X. Value type: %zu. Register count: %u. Value: %0.1f.", + server_register->address, static_cast(server_register->value_type), + server_register->register_count, value); + number_to_payload(sixteen_bit_response, value, server_register->value_type); + current_address += server_register->register_count; + found = true; + break; + } + } + + if (!found) { + ESP_LOGW(TAG, "Could not match any register to address %02X. Sending exception response.", current_address); + std::vector error_response; + error_response.push_back(this->address_); + error_response.push_back(0x81); + error_response.push_back(0x02); + this->send_raw(error_response); + return; + } + } + + std::vector response; + for (auto v : sixteen_bit_response) { + auto decoded_value = decode_value(v); + response.push_back(decoded_value[0]); + response.push_back(decoded_value[1]); + } + + this->send(function_code, start_address, number_of_registers, response.size(), response.data()); +} + SensorSet ModbusController::find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const { auto reg_it = find_if(begin(register_ranges_), end(register_ranges_), [=](RegisterRange const &r) { return (r.start_address == start_address && r.register_type == register_type); @@ -190,7 +232,7 @@ void ModbusController::update() { // walk through the sensors and determine the register ranges to read size_t ModbusController::create_register_ranges_() { register_ranges_.clear(); - if (sensorset_.empty()) { + if (this->parent_->role == modbus::ModbusRole::CLIENT && sensorset_.empty()) { ESP_LOGW(TAG, "No sensors registered"); return 0; } @@ -309,6 +351,11 @@ void ModbusController::dump_config() { ESP_LOGCONFIG(TAG, " Range type=%zu start=0x%X count=%d skip_updates=%d", static_cast(it.register_type), it.start_address, it.register_count, it.skip_updates); } + ESP_LOGCONFIG(TAG, "server registers"); + for (auto &r : server_registers_) { + ESP_LOGCONFIG(TAG, " Address=0x%02X value_type=%zu register_count=%u", r->address, + static_cast(r->value_type), r->register_count); + } #endif } diff --git a/esphome/components/modbus_controller/modbus_controller.h b/esphome/components/modbus_controller/modbus_controller.h index a389375523..9b7d59c93f 100644 --- a/esphome/components/modbus_controller/modbus_controller.h +++ b/esphome/components/modbus_controller/modbus_controller.h @@ -8,6 +8,7 @@ #include #include #include +#include #include namespace esphome { @@ -251,6 +252,21 @@ class SensorItem { bool force_new_range{false}; }; +class ServerRegister { + public: + ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count, + std::function read_lambda) { + this->address = address; + this->value_type = value_type; + this->register_count = register_count; + this->read_lambda = std::move(read_lambda); + } + uint16_t address; + SensorValueType value_type; + uint8_t register_count; + std::function read_lambda; +}; + // ModbusController::create_register_ranges_ tries to optimize register range // for this the sensors must be ordered by register_type, start_address and bitmask class SensorItemsComparator { @@ -418,10 +434,14 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice { void queue_command(const ModbusCommandItem &command); /// Registers a sensor with the controller. Called by esphomes code generator void add_sensor_item(SensorItem *item) { sensorset_.insert(item); } + /// Registers a server register with the controller. Called by esphomes code generator + void add_server_register(ServerRegister *server_register) { server_registers_.push_back(server_register); } /// called when a modbus response was parsed without errors void on_modbus_data(const std::vector &data) override; /// called when a modbus error response was received void on_modbus_error(uint8_t function_code, uint8_t exception_code) override; + /// called when a modbus request (function code 3 or 4) was parsed without errors + void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers) final; /// default delegate called by process_modbus_data when a response has retrieved from the incoming queue void on_register_data(ModbusRegisterType register_type, uint16_t start_address, const std::vector &data); /// default delegate called by process_modbus_data when a response for a write response has retrieved from the @@ -452,6 +472,8 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice { void dump_sensors_(); /// Collection of all sensors for this component SensorSet sensorset_; + /// Collection of all server registers for this component + std::vector server_registers_; /// Continuous range of modbus registers std::vector register_ranges_; /// Hold the pending requests to be sent diff --git a/tests/components/modbus_controller/test.esp32.yaml b/tests/components/modbus_controller/test.esp32.yaml index c5fe3fd057..3e022b10ab 100644 --- a/tests/components/modbus_controller/test.esp32.yaml +++ b/tests/components/modbus_controller/test.esp32.yaml @@ -1,14 +1,30 @@ uart: - - id: uart_modbus + - id: uart_modbus_client tx_pin: 17 rx_pin: 16 baud_rate: 9600 + - id: uart_modbus_server + tx_pin: 1 + rx_pin: 3 + baud_rate: 9600 modbus: - id: mod_bus1 - flow_control_pin: 15 + - id: mod_bus1 + uart_id: uart_modbus_client + flow_control_pin: 15 + - id: mod_bus2 + uart_id: uart_modbus_server + role: server modbus_controller: - id: modbus_controller1 address: 0x2 modbus_id: mod_bus1 + - id: modbus_controller2 + address: 0x2 + modbus_id: mod_bus2 + server_registers: + - address: 0x0000 + value_type: S_DWORD_R + read_lambda: |- + return 42.3;