From df73170e5ae387960baeb8a27c14d962bad00119 Mon Sep 17 00:00:00 2001
From: Javier Peletier <jpeletier@users.noreply.github.com>
Date: Sun, 24 Jul 2022 22:06:18 +0200
Subject: [PATCH] modbus: fix queue deduplicator erasing custom commands
 (#3650)

---
 .../modbus_controller/modbus_controller.cpp          | 12 ++++++++++--
 .../components/modbus_controller/modbus_controller.h |  2 ++
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp
index 91e0dcc45f..b7fde157d8 100644
--- a/esphome/components/modbus_controller/modbus_controller.cpp
+++ b/esphome/components/modbus_controller/modbus_controller.cpp
@@ -108,8 +108,7 @@ void ModbusController::queue_command(const ModbusCommandItem &command) {
   // check if this commmand is already qeued.
   // not very effective but the queue is never really large
   for (auto &item : command_queue_) {
-    if (item->register_address == command.register_address && item->register_count == command.register_count &&
-        item->register_type == command.register_type && item->function_code == command.function_code) {
+    if (item->is_equal(command)) {
       ESP_LOGW(TAG, "Duplicate modbus command found: type=0x%x address=%u count=%u",
                static_cast<uint8_t>(command.register_type), command.register_address, command.register_count);
       // update the payload of the queued command
@@ -489,6 +488,15 @@ bool ModbusCommandItem::send() {
   return true;
 }
 
+bool ModbusCommandItem::is_equal(const ModbusCommandItem &other) {
+  // for custom commands we have to check for identical payloads, since
+  // address/count/type fields will be set to zero
+  return this->function_code == ModbusFunctionCode::CUSTOM
+             ? this->payload == other.payload
+             : other.register_address == this->register_address && other.register_count == this->register_count &&
+                   other.register_type == this->register_type && other.function_code == this->function_code;
+}
+
 void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type) {
   switch (value_type) {
     case SensorValueType::U_WORD:
diff --git a/esphome/components/modbus_controller/modbus_controller.h b/esphome/components/modbus_controller/modbus_controller.h
index 6aecf7f8a4..f67242a68e 100644
--- a/esphome/components/modbus_controller/modbus_controller.h
+++ b/esphome/components/modbus_controller/modbus_controller.h
@@ -395,6 +395,8 @@ class ModbusCommandItem {
       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);
+
+  bool is_equal(const ModbusCommandItem &other);
 };
 
 /** Modbus controller class.