ESPHome  2024.11.0
modbus.cpp
Go to the documentation of this file.
1 #include "modbus.h"
2 #include "esphome/core/log.h"
3 #include "esphome/core/helpers.h"
4 
5 namespace esphome {
6 namespace modbus {
7 
8 static const char *const TAG = "modbus";
9 
10 void Modbus::setup() {
11  if (this->flow_control_pin_ != nullptr) {
12  this->flow_control_pin_->setup();
13  }
14 }
15 void Modbus::loop() {
16  const uint32_t now = millis();
17 
18  while (this->available()) {
19  uint8_t byte;
20  this->read_byte(&byte);
21  if (this->parse_modbus_byte_(byte)) {
22  this->last_modbus_byte_ = now;
23  } else {
24  size_t at = this->rx_buffer_.size();
25  if (at > 0) {
26  ESP_LOGV(TAG, "Clearing buffer of %d bytes - parse failed", at);
27  this->rx_buffer_.clear();
28  }
29  }
30  }
31 
32  if (now - this->last_modbus_byte_ > 50) {
33  size_t at = this->rx_buffer_.size();
34  if (at > 0) {
35  ESP_LOGV(TAG, "Clearing buffer of %d bytes - timeout", at);
36  this->rx_buffer_.clear();
37  }
38 
39  // stop blocking new send commands after sent_wait_time_ ms after response received
40  if (now - this->last_send_ > send_wait_time_) {
41  if (waiting_for_response > 0)
42  ESP_LOGV(TAG, "Stop waiting for response from %d", waiting_for_response);
44  }
45  }
46 }
47 
48 bool Modbus::parse_modbus_byte_(uint8_t byte) {
49  size_t at = this->rx_buffer_.size();
50  this->rx_buffer_.push_back(byte);
51  const uint8_t *raw = &this->rx_buffer_[0];
52  ESP_LOGVV(TAG, "Modbus received Byte %d (0X%x)", byte, byte);
53  // Byte 0: modbus address (match all)
54  if (at == 0)
55  return true;
56  uint8_t address = raw[0];
57  uint8_t function_code = raw[1];
58  // Byte 2: Size (with modbus rtu function code 4/3)
59  // See also https://en.wikipedia.org/wiki/Modbus
60  if (at == 2)
61  return true;
62 
63  uint8_t data_len = raw[2];
64  uint8_t data_offset = 3;
65 
66  // Per https://modbus.org/docs/Modbus_Application_Protocol_V1_1b3.pdf Ch 5 User-Defined function codes
67  if (((function_code >= 65) && (function_code <= 72)) || ((function_code >= 100) && (function_code <= 110))) {
68  // Handle user-defined function, since we don't know how big this ought to be,
69  // ideally we should delegate the entire length detection to whatever handler is
70  // installed, but wait, there is the CRC, and if we get a hit there is a good
71  // chance that this is a complete message ... admittedly there is a small chance is
72  // isn't but that is quite small given the purpose of the CRC in the first place
73 
74  // Fewer than 2 bytes can't calc CRC
75  if (at < 2)
76  return true;
77 
78  data_len = at - 2;
79  data_offset = 1;
80 
81  uint16_t computed_crc = crc16(raw, data_offset + data_len);
82  uint16_t remote_crc = uint16_t(raw[data_offset + data_len]) | (uint16_t(raw[data_offset + data_len + 1]) << 8);
83 
84  if (computed_crc != remote_crc)
85  return true;
86 
87  ESP_LOGD(TAG, "Modbus user-defined function %02X found", function_code);
88 
89  } else {
90  // data starts at 2 and length is 4 for read registers commands
91  if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) {
92  data_offset = 2;
93  data_len = 4;
94  }
95 
96  // the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands
97  if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) {
98  data_offset = 2;
99  data_len = 4;
100  }
101 
102  // Error ( msb indicates error )
103  // response format: Byte[0] = device address, Byte[1] function code | 0x80 , Byte[2] exception code, Byte[3-4] crc
104  if ((function_code & 0x80) == 0x80) {
105  data_offset = 2;
106  data_len = 1;
107  }
108 
109  // Byte data_offset..data_offset+data_len-1: Data
110  if (at < data_offset + data_len)
111  return true;
112 
113  // Byte 3+data_len: CRC_LO (over all bytes)
114  if (at == data_offset + data_len)
115  return true;
116 
117  // Byte data_offset+len+1: CRC_HI (over all bytes)
118  uint16_t computed_crc = crc16(raw, data_offset + data_len);
119  uint16_t remote_crc = uint16_t(raw[data_offset + data_len]) | (uint16_t(raw[data_offset + data_len + 1]) << 8);
120  if (computed_crc != remote_crc) {
121  if (this->disable_crc_) {
122  ESP_LOGD(TAG, "Modbus CRC Check failed, but ignored! %02X!=%02X", computed_crc, remote_crc);
123  } else {
124  ESP_LOGW(TAG, "Modbus CRC Check failed! %02X!=%02X", computed_crc, remote_crc);
125  return false;
126  }
127  }
128  }
129  std::vector<uint8_t> data(this->rx_buffer_.begin() + data_offset, this->rx_buffer_.begin() + data_offset + data_len);
130  bool found = false;
131  for (auto *device : this->devices_) {
132  if (device->address_ == address) {
133  // Is it an error response?
134  if ((function_code & 0x80) == 0x80) {
135  ESP_LOGD(TAG, "Modbus error function code: 0x%X exception: %d", function_code, raw[2]);
136  if (waiting_for_response != 0) {
137  device->on_modbus_error(function_code & 0x7F, raw[2]);
138  } else {
139  // Ignore modbus exception not related to a pending command
140  ESP_LOGD(TAG, "Ignoring Modbus error - not expecting a response");
141  }
142  } else if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) {
143  device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8),
144  uint16_t(data[3]) | (uint16_t(data[2]) << 8));
145  } else {
146  device->on_modbus_data(data);
147  }
148  found = true;
149  }
150  }
152 
153  if (!found) {
154  ESP_LOGW(TAG, "Got Modbus frame from unknown address 0x%02X! ", address);
155  }
156 
157  // reset buffer
158  ESP_LOGV(TAG, "Clearing buffer of %d bytes - parse succeeded", at);
159  this->rx_buffer_.clear();
160  return true;
161 }
162 
164  ESP_LOGCONFIG(TAG, "Modbus:");
165  LOG_PIN(" Flow Control Pin: ", this->flow_control_pin_);
166  ESP_LOGCONFIG(TAG, " Send Wait Time: %d ms", this->send_wait_time_);
167  ESP_LOGCONFIG(TAG, " CRC Disabled: %s", YESNO(this->disable_crc_));
168 }
170  // After UART bus
171  return setup_priority::BUS - 1.0f;
172 }
173 
174 void Modbus::send(uint8_t address, uint8_t function_code, uint16_t start_address, uint16_t number_of_entities,
175  uint8_t payload_len, const uint8_t *payload) {
176  static const size_t MAX_VALUES = 128;
177 
178  // Only check max number of registers for standard function codes
179  // Some devices use non standard codes like 0x43
180  if (number_of_entities > MAX_VALUES && function_code <= 0x10) {
181  ESP_LOGE(TAG, "send too many values %d max=%zu", number_of_entities, MAX_VALUES);
182  return;
183  }
184 
185  std::vector<uint8_t> data;
186  data.push_back(address);
187  data.push_back(function_code);
188  if (this->role == ModbusRole::CLIENT) {
189  data.push_back(start_address >> 8);
190  data.push_back(start_address >> 0);
191  if (function_code != 0x5 && function_code != 0x6) {
192  data.push_back(number_of_entities >> 8);
193  data.push_back(number_of_entities >> 0);
194  }
195  }
196 
197  if (payload != nullptr) {
198  if (this->role == ModbusRole::SERVER || function_code == 0xF || function_code == 0x10) { // Write multiple
199  data.push_back(payload_len); // Byte count is required for write
200  } else {
201  payload_len = 2; // Write single register or coil
202  }
203  for (int i = 0; i < payload_len; i++) {
204  data.push_back(payload[i]);
205  }
206  }
207 
208  auto crc = crc16(data.data(), data.size());
209  data.push_back(crc >> 0);
210  data.push_back(crc >> 8);
211 
212  if (this->flow_control_pin_ != nullptr)
213  this->flow_control_pin_->digital_write(true);
214 
215  this->write_array(data);
216  this->flush();
217 
218  if (this->flow_control_pin_ != nullptr)
219  this->flow_control_pin_->digital_write(false);
221  last_send_ = millis();
222  ESP_LOGV(TAG, "Modbus write: %s", format_hex_pretty(data).c_str());
223 }
224 
225 // Helper function for lambdas
226 // Send raw command. Except CRC everything must be contained in payload
227 void Modbus::send_raw(const std::vector<uint8_t> &payload) {
228  if (payload.empty()) {
229  return;
230  }
231 
232  if (this->flow_control_pin_ != nullptr)
233  this->flow_control_pin_->digital_write(true);
234 
235  auto crc = crc16(payload.data(), payload.size());
236  this->write_array(payload);
237  this->write_byte(crc & 0xFF);
238  this->write_byte((crc >> 8) & 0xFF);
239  this->flush();
240  if (this->flow_control_pin_ != nullptr)
241  this->flow_control_pin_->digital_write(false);
242  waiting_for_response = payload[0];
243  ESP_LOGV(TAG, "Modbus write raw: %s", format_hex_pretty(payload).c_str());
244  last_send_ = millis();
245 }
246 
247 } // namespace modbus
248 } // namespace esphome
virtual void digital_write(bool value)=0
uint8_t raw[35]
Definition: bl0939.h:19
std::string format_hex_pretty(const uint8_t *data, size_t length)
Format the byte array data of length len in pretty-printed, human-readable hex.
Definition: helpers.cpp:364
void write_array(const uint8_t *data, size_t len)
Definition: uart.h:21
std::vector< uint8_t > rx_buffer_
Definition: modbus.h:49
bool parse_modbus_byte_(uint8_t byte)
Definition: modbus.cpp:48
void write_byte(uint8_t data)
Definition: uart.h:19
uint32_t last_send_
Definition: modbus.h:51
void send_raw(const std::vector< uint8_t > &payload)
Definition: modbus.cpp:227
void send(uint8_t address, uint8_t function_code, uint16_t start_address, uint16_t number_of_entities, uint8_t payload_len=0, const uint8_t *payload=nullptr)
Definition: modbus.cpp:174
std::vector< ModbusDevice * > devices_
Definition: modbus.h:52
float get_setup_priority() const override
Definition: modbus.cpp:169
virtual void setup()=0
GPIOPin * flow_control_pin_
Definition: modbus.h:44
uint16_t crc16(const uint8_t *data, uint16_t len, uint16_t crc, uint16_t reverse_poly, bool refin, bool refout)
Calculate a CRC-16 checksum of data with size len.
Definition: helpers.cpp:111
uint32_t IRAM_ATTR HOT millis()
Definition: core.cpp:25
void dump_config() override
Definition: modbus.cpp:163
ModbusRole role
Definition: modbus.h:41
const float BUS
For communication buses like i2c/spi.
Definition: component.cpp:16
bool read_byte(uint8_t *data)
Definition: uart.h:29
uint16_t send_wait_time_
Definition: modbus.h:47
uint32_t last_modbus_byte_
Definition: modbus.h:50
uint8_t waiting_for_response
Definition: modbus.h:37
void loop() override
Definition: modbus.cpp:15
Implementation of SPI Controller mode.
Definition: a01nyub.cpp:7
uint8_t address
Definition: bl0906.h:211
void setup() override
Definition: modbus.cpp:10