ESPHome  2022.11.3
modbus_controller.h
Go to the documentation of this file.
1 #pragma once
2 
4 
7 
8 #include <list>
9 #include <queue>
10 #include <set>
11 #include <vector>
12 
13 namespace esphome {
14 namespace modbus_controller {
15 
16 class ModbusController;
17 
18 enum class ModbusFunctionCode {
19  CUSTOM = 0x00,
20  READ_COILS = 0x01,
21  READ_DISCRETE_INPUTS = 0x02,
23  READ_INPUT_REGISTERS = 0x04,
24  WRITE_SINGLE_COIL = 0x05,
25  WRITE_SINGLE_REGISTER = 0x06,
26  READ_EXCEPTION_STATUS = 0x07, // not implemented
27  DIAGNOSTICS = 0x08, // not implemented
28  GET_COMM_EVENT_COUNTER = 0x0B, // not implemented
29  GET_COMM_EVENT_LOG = 0x0C, // not implemented
30  WRITE_MULTIPLE_COILS = 0x0F,
32  REPORT_SERVER_ID = 0x11, // not implemented
33  READ_FILE_RECORD = 0x14, // not implemented
34  WRITE_FILE_RECORD = 0x15, // not implemented
35  MASK_WRITE_REGISTER = 0x16, // not implemented
36  READ_WRITE_MULTIPLE_REGISTERS = 0x17, // not implemented
37  READ_FIFO_QUEUE = 0x18, // not implemented
38 };
39 
40 enum class ModbusRegisterType : uint8_t {
41  CUSTOM = 0x0,
42  COIL = 0x01,
43  DISCRETE_INPUT = 0x02,
44  HOLDING = 0x03,
45  READ = 0x04,
46 };
47 
48 enum class SensorValueType : uint8_t {
49  RAW = 0x00, // variable length
50  U_WORD = 0x1, // 1 Register unsigned
51  U_DWORD = 0x2, // 2 Registers unsigned
52  S_WORD = 0x3, // 1 Register signed
53  S_DWORD = 0x4, // 2 Registers signed
54  BIT = 0x5,
55  U_DWORD_R = 0x6, // 2 Registers unsigned
56  S_DWORD_R = 0x7, // 2 Registers unsigned
57  U_QWORD = 0x8,
58  S_QWORD = 0x9,
59  U_QWORD_R = 0xA,
60  S_QWORD_R = 0xB,
61  FP32 = 0xC,
62  FP32_R = 0xD
63 };
64 
66  switch (reg_type) {
69  break;
72  break;
75  break;
78  break;
79  default:
81  break;
82  }
83 }
85  switch (reg_type) {
88  break;
91  break;
94  break;
96  default:
98  break;
99  }
100 }
101 
102 inline uint8_t c_to_hex(char c) { return (c >= 'A') ? (c >= 'a') ? (c - 'a' + 10) : (c - 'A' + 10) : (c - '0'); }
103 
112 inline uint8_t byte_from_hex_str(const std::string &value, uint8_t pos) {
113  if (value.length() < pos * 2 + 1)
114  return 0;
115  return (c_to_hex(value[pos * 2]) << 4) | c_to_hex(value[pos * 2 + 1]);
116 }
117 
124 inline uint16_t word_from_hex_str(const std::string &value, uint8_t pos) {
125  return byte_from_hex_str(value, pos) << 8 | byte_from_hex_str(value, pos + 1);
126 }
127 
134 inline uint32_t dword_from_hex_str(const std::string &value, uint8_t pos) {
135  return word_from_hex_str(value, pos) << 16 | word_from_hex_str(value, pos + 2);
136 }
137 
144 inline uint64_t qword_from_hex_str(const std::string &value, uint8_t pos) {
145  return static_cast<uint64_t>(dword_from_hex_str(value, pos)) << 32 | dword_from_hex_str(value, pos + 4);
146 }
147 
148 // Extract data from modbus response buffer
155 template<typename T> T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
156  if (sizeof(T) == sizeof(uint8_t)) {
157  return T(data[buffer_offset]);
158  }
159  if (sizeof(T) == sizeof(uint16_t)) {
160  return T((uint16_t(data[buffer_offset + 0]) << 8) | (uint16_t(data[buffer_offset + 1]) << 0));
161  }
162 
163  if (sizeof(T) == sizeof(uint32_t)) {
164  return get_data<uint16_t>(data, buffer_offset) << 16 | get_data<uint16_t>(data, (buffer_offset + 2));
165  }
166 
167  if (sizeof(T) == sizeof(uint64_t)) {
168  return static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset)) << 32 |
169  (static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset + 4)));
170  }
171 }
172 
181 inline bool coil_from_vector(int coil, const std::vector<uint8_t> &data) {
182  auto data_byte = coil / 8;
183  return (data[data_byte] & (1 << (coil % 8))) > 0;
184 }
185 
196 template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
197  auto result = (mask & data);
198  if (result == 0 || mask == 0xFFFFFFFF) {
199  return result;
200  }
201  for (size_t pos = 0; pos < sizeof(N) << 3; pos++) {
202  if ((mask & (1 << pos)) != 0)
203  return result >> pos;
204  }
205  return 0;
206 }
207 
214 void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type);
215 
223 int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
224  uint32_t bitmask);
225 
226 class ModbusController;
227 
228 class SensorItem {
229  public:
230  virtual void parse_and_publish(const std::vector<uint8_t> &data) = 0;
231 
232  void set_custom_data(const std::vector<uint8_t> &data) { custom_data = data; }
233  size_t virtual get_register_size() const {
234  if (register_type == ModbusRegisterType::COIL || register_type == ModbusRegisterType::DISCRETE_INPUT) {
235  return 1;
236  } else { // if CONF_RESPONSE_BYTES is used override the default
237  return response_bytes > 0 ? response_bytes : register_count * 2;
238  }
239  }
240  // Override register size for modbus devices not using 1 register for one dword
241  void set_register_size(uint8_t register_size) { response_bytes = register_size; }
244  uint16_t start_address;
245  uint32_t bitmask;
246  uint8_t offset;
247  uint8_t register_count;
248  uint8_t response_bytes{0};
249  uint8_t skip_updates;
250  std::vector<uint8_t> custom_data{};
251  bool force_new_range{false};
252 };
253 
254 // ModbusController::create_register_ranges_ tries to optimize register range
255 // for this the sensors must be ordered by register_type, start_address and bitmask
257  public:
258  bool operator()(const SensorItem *lhs, const SensorItem *rhs) const {
259  // first sort according to register type
260  if (lhs->register_type != rhs->register_type) {
261  return lhs->register_type < rhs->register_type;
262  }
263 
264  // ensure that sensor with force_new_range set are before the others
265  if (lhs->force_new_range != rhs->force_new_range) {
266  return lhs->force_new_range > rhs->force_new_range;
267  }
268 
269  // sort by start address
270  if (lhs->start_address != rhs->start_address) {
271  return lhs->start_address < rhs->start_address;
272  }
273 
274  // sort by offset (ensures update of sensors in ascending order)
275  if (lhs->offset != rhs->offset) {
276  return lhs->offset < rhs->offset;
277  }
278 
279  // The pointer to the sensor is used last to ensure that
280  // multiple sensors with the same values can be added with a stable sort order.
281  return lhs < rhs;
282  }
283 };
284 
285 using SensorSet = std::set<SensorItem *, SensorItemsComparator>;
286 
288  uint16_t start_address;
290  uint8_t register_count;
291  uint8_t skip_updates; // the config value
292  SensorSet sensors; // all sensors of this range
293  uint8_t skip_updates_counter; // the running value
294 };
295 
297  public:
298  static const size_t MAX_PAYLOAD_BYTES = 240;
299  static const uint8_t MAX_SEND_REPEATS = 5;
302  uint16_t register_count;
305  std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
306  on_data_func;
307  std::vector<uint8_t> payload = {};
308  bool send();
309  // wrong commands (esp. custom commands) can block the send queue
310  // limit the number of repeats
311  uint8_t send_countdown{MAX_SEND_REPEATS};
313 
322  static ModbusCommandItem create_read_command(
323  ModbusController *modbusdevice, ModbusRegisterType register_type, uint16_t start_address, uint16_t register_count,
324  std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
325  &&handler);
334  static ModbusCommandItem create_read_command(ModbusController *modbusdevice, ModbusRegisterType register_type,
335  uint16_t start_address, uint16_t register_count);
345  static ModbusCommandItem create_write_multiple_command(ModbusController *modbusdevice, uint16_t start_address,
346  uint16_t register_count, const std::vector<uint16_t> &values);
355  static ModbusCommandItem create_write_single_command(ModbusController *modbusdevice, uint16_t start_address,
356  uint16_t value);
364  static ModbusCommandItem create_write_single_coil(ModbusController *modbusdevice, uint16_t address, bool value);
365 
373  static ModbusCommandItem create_write_multiple_coils(ModbusController *modbusdevice, uint16_t start_address,
374  const std::vector<bool> &values);
382  static ModbusCommandItem create_custom_command(
383  ModbusController *modbusdevice, const std::vector<uint8_t> &values,
384  std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
385  &&handler = nullptr);
386 
394  static ModbusCommandItem create_custom_command(
395  ModbusController *modbusdevice, const std::vector<uint16_t> &values,
396  std::function<void(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data)>
397  &&handler = nullptr);
398 
399  bool is_equal(const ModbusCommandItem &other);
400 };
401 
411  public:
412  ModbusController(uint16_t throttle = 0) : command_throttle_(throttle){};
413  void dump_config() override;
414  void loop() override;
415  void setup() override;
416  void update() override;
417 
419  void queue_command(const ModbusCommandItem &command);
421  void add_sensor_item(SensorItem *item) { sensorset_.insert(item); }
423  void on_modbus_data(const std::vector<uint8_t> &data) override;
425  void on_modbus_error(uint8_t function_code, uint8_t exception_code) override;
427  void on_register_data(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data);
430  void on_write_register_response(ModbusRegisterType register_type, uint16_t start_address,
431  const std::vector<uint8_t> &data);
433  void set_command_throttle(uint16_t command_throttle) { this->command_throttle_ = command_throttle; }
434 
435  protected:
437  size_t create_register_ranges_();
438  // find register in sensormap. Returns iterator with all registers having the same start address
439  SensorSet find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const;
441  void update_range_(RegisterRange &r);
443  void process_modbus_data_(const ModbusCommandItem *response);
445  bool send_next_command_();
447  size_t get_command_queue_length_() { return command_queue_.size(); }
449  void dump_sensors_();
453  std::vector<RegisterRange> register_ranges_;
455  std::list<std::unique_ptr<ModbusCommandItem>> command_queue_;
457  std::queue<std::unique_ptr<ModbusCommandItem>> incoming_queue_;
462 };
463 
469 inline float payload_to_float(const std::vector<uint8_t> &data, const SensorItem &item) {
470  int64_t number = payload_to_number(data, item.sensor_value_type, item.offset, item.bitmask);
471 
472  float float_value;
474  float_value = bit_cast<float>(static_cast<uint32_t>(number));
475  } else {
476  float_value = static_cast<float>(number);
477  }
478 
479  return float_value;
480 }
481 
482 inline std::vector<uint16_t> float_to_payload(float value, SensorValueType value_type) {
483  int64_t val;
484 
485  if (value_type == SensorValueType::FP32 || value_type == SensorValueType::FP32_R) {
486  val = bit_cast<uint32_t>(value);
487  } else {
488  val = llroundf(value);
489  }
490 
491  std::vector<uint16_t> data;
492  number_to_payload(data, val, value_type);
493  return data;
494 }
495 
496 } // namespace modbus_controller
497 } // namespace esphome
void setup()
void loop()
bool operator()(const SensorItem *lhs, const SensorItem *rhs) const
uint16_t word_from_hex_str(const std::string &value, uint8_t pos)
Get a word from a hex string.
N mask_and_shift_by_rightbit(N data, uint32_t mask)
Extract bits from value and shift right according to the bitmask if the bitmask is 0x00F0 we want the...
std::vector< uint16_t > float_to_payload(float value, SensorValueType value_type)
uint16_t command_throttle_
min time in ms between sending modbus commands
This class simplifies creating components that periodically check a state.
Definition: component.h:267
T get_data(const std::vector< uint8_t > &data, size_t buffer_offset)
Extract data from modbus response buffer.
uint32_t last_command_timestamp_
when was the last send operation
bool coil_from_vector(int coil, const std::vector< uint8_t > &data)
Extract coil data from modbus response buffer Responses for coil are packed into bytes ...
void set_register_size(uint8_t register_size)
SensorSet sensorset_
Collection of all sensors for this component.
uint64_t qword_from_hex_str(const std::string &value, uint8_t pos)
Get a qword from a hex string.
uint32_t dword_from_hex_str(const std::string &value, uint8_t pos)
Get a dword from a hex string.
float payload_to_float(const std::vector< uint8_t > &data, const SensorItem &item)
Convert vector<uint8_t> response payload to float.
void number_to_payload(std::vector< uint16_t > &data, int64_t value, SensorValueType value_type)
Convert float value to vector<uint16_t> suitable for sending.
uint8_t byte_from_hex_str(const std::string &value, uint8_t pos)
Get a byte from a hex string hex_byte_from_str("1122",1) returns uint_8 value 0x22 == 34 hex_byte_fro...
void add_sensor_item(SensorItem *item)
Registers a sensor with the controller. Called by esphomes code generator.
void set_command_throttle(uint16_t command_throttle)
called by esphome generated code to set the command_throttle period
std::list< std::unique_ptr< ModbusCommandItem > > command_queue_
Hold the pending requests to be sent.
ModbusFunctionCode modbus_register_write_function(ModbusRegisterType reg_type)
ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type)
size_t get_command_queue_length_()
get the number of queued modbus commands (should be mostly empty)
int64_t payload_to_number(const std::vector< uint8_t > &data, SensorValueType sensor_value_type, uint8_t offset, uint32_t bitmask)
Convert vector<uint8_t> response payload to number.
std::vector< RegisterRange > register_ranges_
Continuous range of modbus registers.
To bit_cast(const From &src)
Convert data between types, without aliasing issues or undefined behaviour.
Definition: helpers.h:113
void set_custom_data(const std::vector< uint8_t > &data)
Definition: a4988.cpp:4
uint32_t val
Definition: datatypes.h:85
std::set< SensorItem *, SensorItemsComparator > SensorSet
std::queue< std::unique_ptr< ModbusCommandItem > > incoming_queue_
modbus response data waiting to get processed