From 2df0ebd895220f8f8fd0b37bb929749f47f1a6ab Mon Sep 17 00:00:00 2001 From: Javier Peletier Date: Wed, 25 Jun 2025 01:31:23 +0200 Subject: [PATCH] [modbus_controller] Fix modbus read_lambda precision for non-floats or large integers (#9159) --- esphome/components/modbus/modbus.h | 8 +++ .../components/modbus_controller/__init__.py | 43 ++++++++++----- .../modbus_controller/modbus_controller.cpp | 21 ++++---- .../modbus_controller/modbus_controller.h | 53 ++++++++++++++++--- 4 files changed, 96 insertions(+), 29 deletions(-) diff --git a/esphome/components/modbus/modbus.h b/esphome/components/modbus/modbus.h index 4a78ed4aab..aebdbccc78 100644 --- a/esphome/components/modbus/modbus.h +++ b/esphome/components/modbus/modbus.h @@ -64,6 +64,14 @@ class ModbusDevice { this->parent_->send(this->address_, function, start_address, number_of_entities, payload_len, payload); } void send_raw(const std::vector &payload) { this->parent_->send_raw(payload); } + void send_error(uint8_t function_code, uint8_t exception_code) { + std::vector error_response; + error_response.reserve(3); + error_response.push_back(this->address_); + error_response.push_back(function_code | 0x80); + error_response.push_back(exception_code); + this->send_raw(error_response); + } // If more than one device is connected block sending a new command before a response is received bool waiting_for_response() { return parent_->waiting_for_response != 0; } diff --git a/esphome/components/modbus_controller/__init__.py b/esphome/components/modbus_controller/__init__.py index 61b60498d0..8079b824b0 100644 --- a/esphome/components/modbus_controller/__init__.py +++ b/esphome/components/modbus_controller/__init__.py @@ -112,6 +112,22 @@ TYPE_REGISTER_MAP = { "FP32_R": 2, } +CPP_TYPE_REGISTER_MAP = { + "RAW": cg.uint16, + "U_WORD": cg.uint16, + "S_WORD": cg.int16, + "U_DWORD": cg.uint32, + "U_DWORD_R": cg.uint32, + "S_DWORD": cg.int32, + "S_DWORD_R": cg.int32, + "U_QWORD": cg.uint64, + "U_QWORD_R": cg.uint64, + "S_QWORD": cg.int64, + "S_QWORD_R": cg.int64, + "FP32": cg.float_, + "FP32_R": cg.float_, +} + ModbusCommandSentTrigger = modbus_controller_ns.class_( "ModbusCommandSentTrigger", automation.Trigger.template(cg.int_, cg.int_) ) @@ -285,21 +301,24 @@ async def to_code(config): 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]: + server_register_var = cg.new_Pvariable( + server_register[CONF_ID], + server_register[CONF_ADDRESS], + server_register[CONF_VALUE_TYPE], + TYPE_REGISTER_MAP[server_register[CONF_VALUE_TYPE]], + ) + cpp_type = CPP_TYPE_REGISTER_MAP[server_register[CONF_VALUE_TYPE]] 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_, - ), - ) + server_register_var.set_read_lambda( + cg.TemplateArguments(cpp_type), + await cg.process_lambda( + server_register[CONF_READ_LAMBDA], + [(cg.uint16, "address")], + return_type=cpp_type, + ), ) ) + cg.add(var.add_server_register(server_register_var)) await register_modbus_device(var, config) for conf in config.get(CONF_ON_COMMAND_SENT, []): trigger = cg.new_Pvariable(conf[CONF_TRIGGER_ID], var) diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp index 48ff868087..81e9ccf0a6 100644 --- a/esphome/components/modbus_controller/modbus_controller.cpp +++ b/esphome/components/modbus_controller/modbus_controller.cpp @@ -117,12 +117,17 @@ void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t bool found = false; for (auto *server_register : this->server_registers_) { if (server_register->address == current_address) { - float value = server_register->read_lambda(); + if (!server_register->read_lambda) { + break; + } + int64_t value = server_register->read_lambda(); + ESP_LOGD(TAG, "Matched register. Address: 0x%02X. Value type: %zu. Register count: %u. Value: %s.", + server_register->address, static_cast(server_register->value_type), + server_register->register_count, server_register->format_value(value).c_str()); - 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); - std::vector payload = float_to_payload(value, server_register->value_type); + std::vector payload; + payload.reserve(server_register->register_count * 2); + number_to_payload(payload, value, server_register->value_type); sixteen_bit_response.insert(sixteen_bit_response.end(), payload.cbegin(), payload.cend()); current_address += server_register->register_count; found = true; @@ -132,11 +137,7 @@ void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t 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); + send_error(function_code, 0x02); return; } } diff --git a/esphome/components/modbus_controller/modbus_controller.h b/esphome/components/modbus_controller/modbus_controller.h index dfd52e44bc..11d27c4025 100644 --- a/esphome/components/modbus_controller/modbus_controller.h +++ b/esphome/components/modbus_controller/modbus_controller.h @@ -63,6 +63,10 @@ enum class SensorValueType : uint8_t { FP32_R = 0xD }; +inline bool value_type_is_float(SensorValueType v) { + return v == SensorValueType::FP32 || v == SensorValueType::FP32_R; +} + inline ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type) { switch (reg_type) { case ModbusRegisterType::COIL: @@ -253,18 +257,53 @@ class SensorItem { }; class ServerRegister { + using ReadLambda = std::function; + public: - ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count, - std::function read_lambda) { + ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count) { this->address = address; this->value_type = value_type; this->register_count = register_count; - this->read_lambda = std::move(read_lambda); } + + template void set_read_lambda(const std::function &&user_read_lambda) { + this->read_lambda = [this, user_read_lambda]() -> int64_t { + T user_value = user_read_lambda(this->address); + if constexpr (std::is_same_v) { + return bit_cast(user_value); + } else { + return static_cast(user_value); + } + }; + } + + // Formats a raw value into a string representation based on the value type for debugging + std::string format_value(int64_t value) const { + switch (this->value_type) { + case SensorValueType::U_WORD: + case SensorValueType::U_DWORD: + case SensorValueType::U_DWORD_R: + case SensorValueType::U_QWORD: + case SensorValueType::U_QWORD_R: + return std::to_string(static_cast(value)); + case SensorValueType::S_WORD: + case SensorValueType::S_DWORD: + case SensorValueType::S_DWORD_R: + case SensorValueType::S_QWORD: + case SensorValueType::S_QWORD_R: + return std::to_string(value); + case SensorValueType::FP32_R: + case SensorValueType::FP32: + return str_sprintf("%.1f", bit_cast(static_cast(value))); + default: + return std::to_string(value); + } + } + uint16_t address{0}; SensorValueType value_type{SensorValueType::RAW}; uint8_t register_count{0}; - std::function read_lambda; + ReadLambda read_lambda; }; // ModbusController::create_register_ranges_ tries to optimize register range @@ -444,7 +483,7 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice { 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 + /// called when a modbus request (function code 0x03 or 0x04) 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); @@ -529,7 +568,7 @@ inline float payload_to_float(const std::vector &data, const SensorItem int64_t number = payload_to_number(data, item.sensor_value_type, item.offset, item.bitmask); float float_value; - if (item.sensor_value_type == SensorValueType::FP32 || item.sensor_value_type == SensorValueType::FP32_R) { + if (value_type_is_float(item.sensor_value_type)) { float_value = bit_cast(static_cast(number)); } else { float_value = static_cast(number); @@ -541,7 +580,7 @@ inline float payload_to_float(const std::vector &data, const SensorItem inline std::vector float_to_payload(float value, SensorValueType value_type) { int64_t val; - if (value_type == SensorValueType::FP32 || value_type == SensorValueType::FP32_R) { + if (value_type_is_float(value_type)) { val = bit_cast(value); } else { val = llroundf(value);