modbus_controller.h

Go to the documentation of this file.

#pragma once

#include "esphome/core/component.h"

#include "esphome/components/modbus/modbus.h"
#include "esphome/core/automation.h"

#include <list>
#include <queue>
#include <set>
#include <utility>
#include <vector>

namespace esphome {
namespace modbus_controller {

class ModbusController;

enum class ModbusFunctionCode {
  CUSTOM = 0x00,
  READ_COILS = 0x01,
  READ_DISCRETE_INPUTS = 0x02,
  READ_HOLDING_REGISTERS = 0x03,
  READ_INPUT_REGISTERS = 0x04,
  WRITE_SINGLE_COIL = 0x05,
  WRITE_SINGLE_REGISTER = 0x06,
  READ_EXCEPTION_STATUS = 0x07,   // not implemented
  DIAGNOSTICS = 0x08,             // not implemented
  GET_COMM_EVENT_COUNTER = 0x0B,  // not implemented
  GET_COMM_EVENT_LOG = 0x0C,      // not implemented
  WRITE_MULTIPLE_COILS = 0x0F,
  WRITE_MULTIPLE_REGISTERS = 0x10,
  REPORT_SERVER_ID = 0x11,               // not implemented
  READ_FILE_RECORD = 0x14,               // not implemented
  WRITE_FILE_RECORD = 0x15,              // not implemented
  MASK_WRITE_REGISTER = 0x16,            // not implemented
  READ_WRITE_MULTIPLE_REGISTERS = 0x17,  // not implemented
  READ_FIFO_QUEUE = 0x18,                // not implemented
};

enum class ModbusRegisterType : uint8_t {
  CUSTOM = 0x0,
  COIL = 0x01,
  DISCRETE_INPUT = 0x02,
  HOLDING = 0x03,
  READ = 0x04,
};

enum class SensorValueType : uint8_t {
  RAW = 0x00,     // variable length
  U_WORD = 0x1,   // 1 Register unsigned
  U_DWORD = 0x2,  // 2 Registers unsigned
  S_WORD = 0x3,   // 1 Register signed
  S_DWORD = 0x4,  // 2 Registers signed
  BIT = 0x5,
  U_DWORD_R = 0x6,  // 2 Registers unsigned
  S_DWORD_R = 0x7,  // 2 Registers unsigned
  U_QWORD = 0x8,
  S_QWORD = 0x9,
  U_QWORD_R = 0xA,
  S_QWORD_R = 0xB,
  FP32 = 0xC,
  FP32_R = 0xD
};

inline ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type) {
  switch (reg_type) {
    case ModbusRegisterType::COIL:
      return ModbusFunctionCode::READ_COILS;
      break;
    case ModbusRegisterType::DISCRETE_INPUT:
      return ModbusFunctionCode::READ_DISCRETE_INPUTS;
      break;
    case ModbusRegisterType::HOLDING:
      return ModbusFunctionCode::READ_HOLDING_REGISTERS;
      break;
    case ModbusRegisterType::READ:
      return ModbusFunctionCode::READ_INPUT_REGISTERS;
      break;
    default:
      return ModbusFunctionCode::CUSTOM;
      break;
  }
}
inline ModbusFunctionCode modbus_register_write_function(ModbusRegisterType reg_type) {
  switch (reg_type) {
    case ModbusRegisterType::COIL:
      return ModbusFunctionCode::WRITE_SINGLE_COIL;
      break;
    case ModbusRegisterType::DISCRETE_INPUT:
      return ModbusFunctionCode::CUSTOM;
      break;
    case ModbusRegisterType::HOLDING:
      return ModbusFunctionCode::READ_WRITE_MULTIPLE_REGISTERS;
      break;
    case ModbusRegisterType::READ:
    default:
      return ModbusFunctionCode::CUSTOM;
      break;
  }
}

inline uint8_t c_to_hex(char c) { return (c >= 'A') ? (c >= 'a') ? (c - 'a' + 10) : (c - 'A' + 10) : (c - '0'); }

inline uint8_t byte_from_hex_str(const std::string &value, uint8_t pos) {
  if (value.length() < pos * 2 + 1)
    return 0;
  return (c_to_hex(value[pos * 2]) << 4) | c_to_hex(value[pos * 2 + 1]);
}

inline uint16_t word_from_hex_str(const std::string &value, uint8_t pos) {
  return byte_from_hex_str(value, pos) << 8 | byte_from_hex_str(value, pos + 1);
}

inline uint32_t dword_from_hex_str(const std::string &value, uint8_t pos) {
  return word_from_hex_str(value, pos) << 16 | word_from_hex_str(value, pos + 2);
}

inline uint64_t qword_from_hex_str(const std::string &value, uint8_t pos) {
  return static_cast<uint64_t>(dword_from_hex_str(value, pos)) << 32 | dword_from_hex_str(value, pos + 4);
}

// Extract data from modbus response buffer
template<typename T> T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
  if (sizeof(T) == sizeof(uint8_t)) {
    return T(data[buffer_offset]);
  }
  if (sizeof(T) == sizeof(uint16_t)) {
    return T((uint16_t(data[buffer_offset + 0]) << 8) | (uint16_t(data[buffer_offset + 1]) << 0));
  }

  if (sizeof(T) == sizeof(uint32_t)) {
    return get_data<uint16_t>(data, buffer_offset) << 16 | get_data<uint16_t>(data, (buffer_offset + 2));
  }

  if (sizeof(T) == sizeof(uint64_t)) {
    return static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset)) << 32 |
           (static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset + 4)));
  }
}

inline bool coil_from_vector(int coil, const std::vector<uint8_t> &data) {
  auto data_byte = coil / 8;
  return (data[data_byte] & (1 << (coil % 8))) > 0;
}

template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
  auto result = (mask & data);
  if (result == 0 || mask == 0xFFFFFFFF) {
    return result;
  }
  for (size_t pos = 0; pos < sizeof(N) << 3; pos++) {
    if ((mask & (1 << pos)) != 0)
      return result >> pos;
  }
  return 0;
}

void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type);

int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
                          uint32_t bitmask);

class ModbusController;

class SensorItem {
 public:
  virtual void parse_and_publish(const std::vector<uint8_t> &data) = 0;

  void set_custom_data(const std::vector<uint8_t> &data) { custom_data = data; }
  size_t virtual get_register_size() const {
    if (register_type == ModbusRegisterType::COIL || register_type == ModbusRegisterType::DISCRETE_INPUT) {
      return 1;
    } else {  // if CONF_RESPONSE_BYTES is used override the default
      return response_bytes > 0 ? response_bytes : register_count * 2;
    }
  }
  // Override register size for modbus devices not using 1 register for one dword
  void set_register_size(uint8_t register_size) { response_bytes = register_size; }
  ModbusRegisterType register_type{ModbusRegisterType::CUSTOM};
  SensorValueType sensor_value_type{SensorValueType::RAW};
  uint16_t start_address{0};
  uint32_t bitmask{0};
  uint8_t offset{0};
  uint8_t register_count{0};
  uint8_t response_bytes{0};
  uint16_t skip_updates{0};
  std::vector<uint8_t> custom_data{};
  bool force_new_range{false};
};

class ServerRegister {
 public:
  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);
  }
  uint16_t address{0};
  SensorValueType value_type{SensorValueType::RAW};
  uint8_t register_count{0};
  std::function<float()> 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 {
 public:
  bool operator()(const SensorItem *lhs, const SensorItem *rhs) const {
    // first sort according to register type
    if (lhs->register_type != rhs->register_type) {
      return lhs->register_type < rhs->register_type;
    }

    // ensure that sensor with force_new_range set are before the others
    if (lhs->force_new_range != rhs->force_new_range) {
      return lhs->force_new_range > rhs->force_new_range;
    }

    // sort by start address
    if (lhs->start_address != rhs->start_address) {
      return lhs->start_address < rhs->start_address;
    }

    // sort by offset (ensures update of sensors in ascending order)
    if (lhs->offset != rhs->offset) {
      return lhs->offset < rhs->offset;
    }

    // The pointer to the sensor is used last to ensure that
    // multiple sensors with the same values can be added with a stable sort order.
    return lhs < rhs;
  }
};

using SensorSet = std::set<SensorItem *, SensorItemsComparator>;

struct RegisterRange {
  uint16_t start_address;
  ModbusRegisterType register_type;
  uint8_t register_count;
  uint16_t skip_updates;          // the config value
  SensorSet sensors;              // all sensors of this range
  uint16_t skip_updates_counter;  // the running value
};

class ModbusCommandItem {
 public:
  static const size_t MAX_PAYLOAD_BYTES = 240;
  ModbusController *modbusdevice{nullptr};
  uint16_t register_address{0};
  uint16_t register_count{0};
  ModbusFunctionCode function_code{ModbusFunctionCode::CUSTOM};
  ModbusRegisterType register_type{ModbusRegisterType::CUSTOM};
  std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
      on_data_func;
  std::vector<uint8_t> payload = {};
  bool send();
  bool should_retry(uint8_t max_retries) { return this->send_count_ <= max_retries; };


  static ModbusCommandItem create_read_command(
      ModbusController *modbusdevice, ModbusRegisterType register_type, uint16_t start_address, uint16_t register_count,
      std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
          &&handler);
  static ModbusCommandItem create_read_command(ModbusController *modbusdevice, ModbusRegisterType register_type,
                                               uint16_t start_address, uint16_t register_count);
  static ModbusCommandItem create_write_multiple_command(ModbusController *modbusdevice, uint16_t start_address,
                                                         uint16_t register_count, const std::vector<uint16_t> &values);
  static ModbusCommandItem create_write_single_command(ModbusController *modbusdevice, uint16_t start_address,
                                                       uint16_t value);
  static ModbusCommandItem create_write_single_coil(ModbusController *modbusdevice, uint16_t address, bool value);

  static ModbusCommandItem create_write_multiple_coils(ModbusController *modbusdevice, uint16_t start_address,
                                                       const std::vector<bool> &values);
  static ModbusCommandItem create_custom_command(
      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);

  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);

  bool is_equal(const ModbusCommandItem &other);

 protected:
  // wrong commands (esp. custom commands) can block the send queue, limit the number of repeats.
  uint8_t send_count_{0};
};

class ModbusController : public PollingComponent, public modbus::ModbusDevice {
 public:
  void dump_config() override;
  void loop() override;
  void setup() override;
  void update() override;

  void queue_command(const ModbusCommandItem &command);
  void add_sensor_item(SensorItem *item) { sensorset_.insert(item); }
  void add_server_register(ServerRegister *server_register) { server_registers_.push_back(server_register); }
  void on_modbus_data(const std::vector<uint8_t> &data) override;
  void on_modbus_error(uint8_t function_code, uint8_t exception_code) override;
  void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers) final;
  void on_register_data(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data);
  void on_write_register_response(ModbusRegisterType register_type, uint16_t start_address,
                                  const std::vector<uint8_t> &data);
  void set_allow_duplicate_commands(bool allow_duplicate_commands) {
    this->allow_duplicate_commands_ = allow_duplicate_commands;
  }
  bool get_allow_duplicate_commands() { return this->allow_duplicate_commands_; }
  void set_command_throttle(uint16_t command_throttle) { this->command_throttle_ = command_throttle; }
  void set_offline_skip_updates(uint16_t offline_skip_updates) { this->offline_skip_updates_ = offline_skip_updates; }
  size_t get_command_queue_length() { return command_queue_.size(); }
  bool get_module_offline() { return module_offline_; }
  void add_on_command_sent_callback(std::function<void(int, int)> &&callback);
  void add_on_online_callback(std::function<void(int, int)> &&callback);
  void add_on_offline_callback(std::function<void(int, int)> &&callback);
  void set_max_cmd_retries(uint8_t max_cmd_retries) { this->max_cmd_retries_ = max_cmd_retries; }
  uint8_t get_max_cmd_retries() { return this->max_cmd_retries_; }

 protected:
  size_t create_register_ranges_();
  // find register in sensormap. Returns iterator with all registers having the same start address
  SensorSet find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const;
  void update_range_(RegisterRange &r);
  void process_modbus_data_(const ModbusCommandItem *response);
  bool send_next_command_();
  void dump_sensors_();
  SensorSet sensorset_;
  std::vector<ServerRegister *> server_registers_{};
  std::vector<RegisterRange> register_ranges_{};
  std::list<std::unique_ptr<ModbusCommandItem>> command_queue_;
  std::queue<std::unique_ptr<ModbusCommandItem>> incoming_queue_;
  bool allow_duplicate_commands_{false};
  uint32_t last_command_timestamp_{0};
  uint16_t command_throttle_{0};
  bool module_offline_{false};
  uint16_t offline_skip_updates_{0};
  uint8_t max_cmd_retries_{4};
  CallbackManager<void(int, int)> command_sent_callback_{};
  CallbackManager<void(int, int)> online_callback_{};
  CallbackManager<void(int, int)> offline_callback_{};
};

inline float payload_to_float(const std::vector<uint8_t> &data, const SensorItem &item) {
  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) {
    float_value = bit_cast<float>(static_cast<uint32_t>(number));
  } else {
    float_value = static_cast<float>(number);
  }

  return float_value;
}

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) {
    val = bit_cast<uint32_t>(value);
  } else {
    val = llroundf(value);
  }

  std::vector<uint16_t> data;
  number_to_payload(data, val, value_type);
  return data;
}

}  // namespace modbus_controller
}  // namespace esphome