Publish custom data when modbus number lambda fills vector (#3295)

esphome/components/modbus_controller/modbus_controller.cpp

@@ -455,6 +455,28 @@ ModbusCommandItem ModbusCommandItem::create_custom_command(
   return cmd;
 }
 
+ModbusCommandItem ModbusCommandItem::create_custom_command(
+    ModbusController *modbusdevice, const std::vector<uint16_t> &values,
+    std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
+        &&handler) {
+  ModbusCommandItem cmd = {};
+  cmd.modbusdevice = modbusdevice;
+  cmd.function_code = ModbusFunctionCode::CUSTOM;
+  if (handler == nullptr) {
+    cmd.on_data_func = [](ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data) {
+      ESP_LOGI(TAG, "Custom Command sent");
+    };
+  } else {
+    cmd.on_data_func = handler;
+  }
+  for (auto v : values) {
+    cmd.payload.push_back((v >> 8) & 0xFF);
+    cmd.payload.push_back(v & 0xFF);
+  }
+
+  return cmd;
+}
+
 bool ModbusCommandItem::send() {
   if (this->function_code != ModbusFunctionCode::CUSTOM) {
     modbusdevice->send(uint8_t(this->function_code), this->register_address, this->register_count, this->payload.size(),

esphome/components/modbus_controller/modbus_controller.h

@@ -2,12 +2,12 @@
 
 #include "esphome/core/component.h"
 
-#include "esphome/core/automation.h"
 #include "esphome/components/modbus/modbus.h"
+#include "esphome/core/automation.h"
 
 #include <list>
-#include <set>
 #include <queue>
+#include <set>
 #include <vector>
 
 namespace esphome {
@@ -374,8 +374,8 @@ class ModbusCommandItem {
                                                        const std::vector<bool> &values);
   /** Create custom modbus command
    * @param modbusdevice pointer to the device to execute the command
-   * @param values byte vector of data to be sent to the device. The compplete payload must be provided with the
+   * @param values byte vector of data to be sent to the device. The complete payload must be provided with the
-   * exception of the crc codess
+   * exception of the crc codes
    * @param handler function called when the response is received. Default is just logging a response
    * @return ModbusCommandItem with the prepared command
    */
@@ -383,6 +383,18 @@ class ModbusCommandItem {
       ModbusController *modbusdevice, const std::vector<uint8_t> &values,
       std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
           &&handler = nullptr);
+
+  /** Create custom modbus command
+   * @param modbusdevice pointer to the device to execute the command
+   * @param values word vector of data to be sent to the device. The complete payload must be provided with the
+   * exception of the crc codes
+   * @param handler function called when the response is received. Default is just logging a response
+   * @return ModbusCommandItem with the prepared command
+   */
+  static ModbusCommandItem create_custom_command(
+      ModbusController *modbusdevice, const std::vector<uint16_t> &values,
+      std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
+          &&handler = nullptr);
 };
 
 /** Modbus controller class.

esphome/components/modbus_controller/number/modbus_number.cpp

@@ -26,6 +26,7 @@ void ModbusNumber::parse_and_publish(const std::vector<uint8_t> &data) {
 }
 
 void ModbusNumber::control(float value) {
+  ModbusCommandItem write_cmd;
   std::vector<uint16_t> data;
   float write_value = value;
   // Is there are lambda configured?
@@ -45,33 +46,39 @@ void ModbusNumber::control(float value) {
     write_value = multiply_by_ * write_value;
   }
 
-  // lambda didn't set payload
+  if (!data.empty()) {
-  if (data.empty()) {
+    ESP_LOGV(TAG, "Modbus Number write raw: %s", format_hex_pretty(data).c_str());
-    data = float_to_payload(write_value, this->sensor_value_type);
+    write_cmd = ModbusCommandItem::create_custom_command(
-  }
+        this->parent_, data,
-
+        [this, write_cmd](ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data) {
-  ESP_LOGD(TAG,
+          this->parent_->on_write_register_response(write_cmd.register_type, this->start_address, data);
-           "Updating register: connected Sensor=%s start address=0x%X register count=%d new value=%.02f (val=%.02f)",
+        });
-           this->get_name().c_str(), this->start_address, this->register_count, value, write_value);
-
-  // Create and send the write command
-  ModbusCommandItem write_cmd;
-  if (this->register_count == 1 && !this->use_write_multiple_) {
-    // since offset is in bytes and a register is 16 bits we get the start by adding offset/2
-    write_cmd =
-        ModbusCommandItem::create_write_single_command(parent_, this->start_address + this->offset / 2, data[0]);
   } else {
-    write_cmd = ModbusCommandItem::create_write_multiple_command(parent_, this->start_address + this->offset / 2,
+    data = float_to_payload(write_value, this->sensor_value_type);
-                                                                 this->register_count, data);
+
+    ESP_LOGD(TAG,
+             "Updating register: connected Sensor=%s start address=0x%X register count=%d new value=%.02f (val=%.02f)",
+             this->get_name().c_str(), this->start_address, this->register_count, value, write_value);
+
+    // Create and send the write command
+    if (this->register_count == 1 && !this->use_write_multiple_) {
+      // since offset is in bytes and a register is 16 bits we get the start by adding offset/2
+      write_cmd =
+          ModbusCommandItem::create_write_single_command(parent_, this->start_address + this->offset / 2, data[0]);
+    } else {
+      write_cmd = ModbusCommandItem::create_write_multiple_command(parent_, this->start_address + this->offset / 2,
+                                                                   this->register_count, data);
+    }
+    // publish new value
+    write_cmd.on_data_func = [this, write_cmd, value](ModbusRegisterType register_type, uint16_t start_address,
+                                                      const std::vector<uint8_t> &data) {
+      // gets called when the write command is ack'd from the device
+      parent_->on_write_register_response(write_cmd.register_type, start_address, data);
+      this->publish_state(value);
+    };
   }
-  // publish new value
-  write_cmd.on_data_func = [this, write_cmd, value](ModbusRegisterType register_type, uint16_t start_address,
-                                                    const std::vector<uint8_t> &data) {
-    // gets called when the write command is ack'd from the device
-    parent_->on_write_register_response(write_cmd.register_type, start_address, data);
-    this->publish_state(value);
-  };
   parent_->queue_command(write_cmd);
+  this->publish_state(value);
 }
 void ModbusNumber::dump_config() { LOG_NUMBER(TAG, "Modbus Number", this); }