[modbus_controller] Fix modbus read_lambda precision for non-floats or large integers (#9159)

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<uint8_t> &payload) { this->parent_->send_raw(payload); }
+  void send_error(uint8_t function_code, uint8_t exception_code) {
+    std::vector<uint8_t> 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; }
 

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]:
-            cg.add(
+            server_register_var = cg.new_Pvariable(
-                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]],
+            )
+            cpp_type = CPP_TYPE_REGISTER_MAP[server_register[CONF_VALUE_TYPE]]
+            cg.add(
+                server_register_var.set_read_lambda(
+                    cg.TemplateArguments(cpp_type),
                     await cg.process_lambda(
                         server_register[CONF_READ_LAMBDA],
-                            [],
+                        [(cg.uint16, "address")],
-                            return_type=cg.float_,
+                        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)

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<size_t>(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.",
+        std::vector<uint16_t> payload;
-                 server_register->address, static_cast<uint8_t>(server_register->value_type),
+        payload.reserve(server_register->register_count * 2);
-                 server_register->register_count, value);
+        number_to_payload(payload, value, server_register->value_type);
-        std::vector<uint16_t> payload = float_to_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<uint8_t> error_response;
+      send_error(function_code, 0x02);
-      error_response.push_back(this->address_);
-      error_response.push_back(0x81);
-      error_response.push_back(0x02);
-      this->send_raw(error_response);
       return;
     }
   }

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<int64_t()>;
+
  public:
-  ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count,
+  ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count) {
-                 std::function<float()> read_lambda) {
     this->address = address;
     this->value_type = value_type;
     this->register_count = register_count;
-    this->read_lambda = std::move(read_lambda);
   }
+
+  template<typename T> void set_read_lambda(const std::function<T(uint16_t address)> &&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<T, float>) {
+        return bit_cast<uint32_t>(user_value);
+      } else {
+        return static_cast<int64_t>(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<uint64_t>(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<float>(static_cast<uint32_t>(value)));
+      default:
+        return std::to_string(value);
+    }
+  }
+
   uint16_t address{0};
   SensorValueType value_type{SensorValueType::RAW};
   uint8_t register_count{0};
-  std::function<float()> 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<uint8_t> &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<uint8_t> &data);
@@ -529,7 +568,7 @@ inline float payload_to_float(const std::vector<uint8_t> &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<float>(static_cast<uint32_t>(number));
   } else {
     float_value = static_cast<float>(number);
@@ -541,7 +580,7 @@ inline float payload_to_float(const std::vector<uint8_t> &data, const SensorItem
 inline std::vector<uint16_t> 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<uint32_t>(value);
   } else {
     val = llroundf(value);