Home-IoT/esphome
1
0
Fork 0
mirror of https://github.com/esphome/esphome.git synced 2024-11-22 23:18:10 +01:00
Code Issues Projects Releases Packages Wiki Activity Actions 9

Fix register ranges in modbus controller (#2981)

Browse source
This commit is contained in:
stegm 2022-01-09 16:24:23 +01:00 committed by GitHub
parent 470071e0b0
commit e4555f6997
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 180 additions and 147 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

248
esphome/components/modbus_controller/modbus_controller.cpp
View file

@ -24,15 +24,22 @@ bool ModbusController::send_next_command_() {
if ((last_send > this->command_throttle_) && !waiting_for_response() && !command_queue_.empty()) { if ((last_send > this->command_throttle_) && !waiting_for_response() && !command_queue_.empty()) {
auto &command = command_queue_.front(); auto &command = command_queue_.front();
ESP_LOGV(TAG, "Sending next modbus command to device %d register 0x%02X count %d", this->address_, // remove from queue if command was sent too often
command->register_address, command->register_count); if (command->send_countdown < 1) {
command->send(); ESP_LOGD(
this->last_command_timestamp_ = millis(); TAG,
// remove from queue if no handler is defined or command was sent too often "Modbus command to device=%d register=0x%02X countdown=%d no response received - removed from send queue",
if (!command->on_data_func || command->send_countdown < 1) { this->address_, command->register_address, command->send_countdown);
ESP_LOGD(TAG, "Modbus command to device=%d register=0x%02X countdown=%d removed from queue after send",
this->address_, command->register_address, command->send_countdown);
command_queue_.pop_front(); command_queue_.pop_front();
} else {
ESP_LOGV(TAG, "Sending next modbus command to device %d register 0x%02X count %d", this->address_,
command->register_address, command->register_count);
command->send();
this->last_command_timestamp_ = millis();
// remove from queue if no handler is defined
if (!command->on_data_func) {
command_queue_.pop_front();
}
} }
} }
return (!command_queue_.empty()); return (!command_queue_.empty());
@ -72,36 +79,28 @@ void ModbusController::on_modbus_error(uint8_t function_code, uint8_t exception_
} }
} }
std::map<uint64_t, SensorItem *>::iterator ModbusController::find_register_(ModbusRegisterType register_type, SensorSet ModbusController::find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const {
uint16_t start_address) { auto reg_it = find_if(begin(register_ranges_), end(register_ranges_), [=](RegisterRange const &r) {
auto vec_it = find_if(begin(register_ranges_), end(register_ranges_), [=](RegisterRange const &r) {
return (r.start_address == start_address && r.register_type == register_type); return (r.start_address == start_address && r.register_type == register_type);
}); });
if (vec_it == register_ranges_.end()) { if (reg_it == register_ranges_.end()) {
ESP_LOGE(TAG, "No matching range for sensor found - start_address : 0x%X", start_address); ESP_LOGE(TAG, "No matching range for sensor found - start_address : 0x%X", start_address);
} else { } else {
auto map_it = sensormap_.find(vec_it->first_sensorkey); return reg_it->sensors;
if (map_it == sensormap_.end()) {
ESP_LOGE(TAG, "No sensor found in at start_address : 0x%X (0x%llX)", start_address, vec_it->first_sensorkey);
} else {
return sensormap_.find(vec_it->first_sensorkey);
}
} }
// not found // not found
return std::end(sensormap_); return {};
} }
void ModbusController::on_register_data(ModbusRegisterType register_type, uint16_t start_address, void ModbusController::on_register_data(ModbusRegisterType register_type, uint16_t start_address,
const std::vector<uint8_t> &data) { const std::vector<uint8_t> &data) {
ESP_LOGV(TAG, "data for register address : 0x%X : ", start_address); ESP_LOGV(TAG, "data for register address : 0x%X : ", start_address);
auto map_it = find_register_(register_type, start_address);
// loop through all sensors with the same start address // loop through all sensors with the same start address
while (map_it != sensormap_.end() && map_it->second->start_address == start_address) { auto sensors = find_sensors_(register_type, start_address);
if (map_it->second->register_type == register_type) { for (auto sensor : sensors) {
map_it->second->parse_and_publish(data); sensor->parse_and_publish(data);
}
map_it++;
} }
} }
@ -127,15 +126,16 @@ void ModbusController::update_range_(RegisterRange &r) {
if (r.skip_updates_counter == 0) { if (r.skip_updates_counter == 0) {
// if a custom command is used the user supplied custom_data is only available in the SensorItem. // if a custom command is used the user supplied custom_data is only available in the SensorItem.
if (r.register_type == ModbusRegisterType::CUSTOM) { if (r.register_type == ModbusRegisterType::CUSTOM) {
auto it = this->find_register_(r.register_type, r.start_address); auto sensors = this->find_sensors_(r.register_type, r.start_address);
if (it != sensormap_.end()) { if (!sensors.empty()) {
auto sensor = sensors.cbegin();
auto command_item = ModbusCommandItem::create_custom_command( auto command_item = ModbusCommandItem::create_custom_command(
this, it->second->custom_data, this, (*sensor)->custom_data,
[this](ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data) { [this](ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data) {
this->on_register_data(ModbusRegisterType::CUSTOM, start_address, data); this->on_register_data(ModbusRegisterType::CUSTOM, start_address, data);
}); });
command_item.register_address = it->second->start_address; command_item.register_address = (*sensor)->start_address;
command_item.register_count = it->second->register_count; command_item.register_count = (*sensor)->register_count;
command_item.function_code = ModbusFunctionCode::CUSTOM; command_item.function_code = ModbusFunctionCode::CUSTOM;
queue_command(command_item); queue_command(command_item);
} }
@ -164,102 +164,110 @@ void ModbusController::update() {
} }
} }
// walk through the sensors and determine the registerranges to read // walk through the sensors and determine the register ranges to read
size_t ModbusController::create_register_ranges_() { size_t ModbusController::create_register_ranges_() {
register_ranges_.clear(); register_ranges_.clear();
uint8_t n = 0; if (sensorset_.empty()) {
if (sensormap_.empty()) { ESP_LOGW(TAG, "No sensors registered");
return 0; return 0;
} }
auto ix = sensormap_.begin(); // iterator is sorted see SensorItemsComparator for details
auto prev = ix; auto ix = sensorset_.begin();
int total_register_count = 0; RegisterRange r = {};
uint16_t current_start_address = ix->second->start_address; uint8_t buffer_offset = 0;
uint8_t buffer_offset = ix->second->offset; SensorItem *prev = nullptr;
uint8_t skip_updates = ix->second->skip_updates; while (ix != sensorset_.end()) {
auto first_sensorkey = ix->second->getkey(); SensorItem *curr = *ix;
total_register_count = 0;
while (ix != sensormap_.end()) {
ESP_LOGV(TAG, "Register: 0x%X %d %d 0x%llx (%d) buffer_offset = %d (0x%X) skip=%u", ix->second->start_address,
ix->second->register_count, ix->second->offset, ix->second->getkey(), total_register_count, buffer_offset,
buffer_offset, ix->second->skip_updates);
// if this is a sequential address based on number of registers and address of previous sensor
// convert to an offset to the previous sensor (address 0x101 becomes address 0x100 offset 2 bytes)
if (!ix->second->force_new_range && total_register_count >= 0 &&
prev->second->register_type == ix->second->register_type &&
prev->second->start_address + total_register_count == ix->second->start_address &&
prev->second->start_address < ix->second->start_address) {
ix->second->start_address = prev->second->start_address;
ix->second->offset += prev->second->offset + prev->second->get_register_size();
// replace entry in sensormap_ ESP_LOGV(TAG, "Register: 0x%X %d %d %d offset=%u skip=%u addr=%p", curr->start_address, curr->register_count,
auto const value = ix->second; curr->offset, curr->get_register_size(), curr->offset, curr->skip_updates, curr);
sensormap_.erase(ix);
sensormap_.insert({value->getkey(), value}); if (r.register_count == 0) {
// move iterator back to new element // this is the first register in range
ix = sensormap_.find(value->getkey()); // next(prev, 1); r.start_address = curr->start_address;
} r.register_count = curr->register_count;
if (current_start_address != ix->second->start_address || r.register_type = curr->register_type;
// ( prev->second->start_address + prev->second->offset != ix->second->start_address) || r.sensors.insert(curr);
ix->second->register_type != prev->second->register_type) { r.skip_updates = curr->skip_updates;
// Difference doesn't match so we have a gap r.skip_updates_counter = 0;
if (n > 0) { buffer_offset = curr->get_register_size();
RegisterRange r;
r.start_address = current_start_address; ESP_LOGV(TAG, "Started new range");
r.register_count = total_register_count;
if (prev->second->register_type == ModbusRegisterType::COIL ||
prev->second->register_type == ModbusRegisterType::DISCRETE_INPUT) {
r.register_count = prev->second->offset + 1;
}
r.register_type = prev->second->register_type;
r.first_sensorkey = first_sensorkey;
r.skip_updates = skip_updates;
r.skip_updates_counter = 0;
ESP_LOGV(TAG, "Add range 0x%X %d skip:%d", r.start_address, r.register_count, r.skip_updates);
register_ranges_.push_back(r);
}
skip_updates = ix->second->skip_updates;
current_start_address = ix->second->start_address;
first_sensorkey = ix->second->getkey();
total_register_count = ix->second->register_count;
buffer_offset = ix->second->offset;
n = 1;
} else { } else {
n++; // this is not the first register in range so it might be possible
if (ix->second->offset != prev->second->offset || n == 1) { // to reuse the last register or extend the current range
total_register_count += ix->second->register_count; if (!curr->force_new_range && r.register_type == curr->register_type &&
buffer_offset += ix->second->get_register_size(); curr->register_type != ModbusRegisterType::CUSTOM) {
if (curr->start_address == (r.start_address + r.register_count - prev->register_count) &&
curr->register_count == prev->register_count && curr->get_register_size() == prev->get_register_size()) {
// this register can re-use the data from the previous register
// remove this sensore because start_address is changed (sort-order)
ix = sensorset_.erase(ix);
curr->start_address = r.start_address;
curr->offset += prev->offset;
sensorset_.insert(curr);
// move iterator backwards because it will be incremented later
ix--;
ESP_LOGV(TAG, "Re-use previous register - change to register: 0x%X %d offset=%u", curr->start_address,
curr->register_count, curr->offset);
} else if (curr->start_address == (r.start_address + r.register_count)) {
// this register can extend the current range
// remove this sensore because start_address is changed (sort-order)
ix = sensorset_.erase(ix);
curr->start_address = r.start_address;
curr->offset += buffer_offset;
buffer_offset += curr->get_register_size();
r.register_count += curr->register_count;
sensorset_.insert(curr);
// move iterator backwards because it will be incremented later
ix--;
ESP_LOGV(TAG, "Extend range - change to register: 0x%X %d offset=%u", curr->start_address,
curr->register_count, curr->offset);
}
} }
}
if (curr->start_address == r.start_address) {
// use the lowest non zero value for the whole range // use the lowest non zero value for the whole range
// Because zero is the default value for skip_updates it is excluded from getting the min value. // Because zero is the default value for skip_updates it is excluded from getting the min value.
if (ix->second->skip_updates != 0) { if (curr->skip_updates != 0) {
if (skip_updates != 0) { if (r.skip_updates != 0) {
skip_updates = std::min(skip_updates, ix->second->skip_updates); r.skip_updates = std::min(r.skip_updates, curr->skip_updates);
} else { } else {
skip_updates = ix->second->skip_updates; r.skip_updates = curr->skip_updates;
} }
} }
// add sensor to this range
r.sensors.insert(curr);
ix++;
} else {
ESP_LOGV(TAG, "Add range 0x%X %d skip:%d", r.start_address, r.register_count, r.skip_updates);
register_ranges_.push_back(r);
r = {};
buffer_offset = 0;
// do not increment the iterator here because the current sensor has to be re-evaluated
} }
prev = ix++;
prev = curr;
} }
// Add the last range
if (n > 0) { if (r.register_count > 0) {
RegisterRange r; // Add the last range
r.start_address = current_start_address;
// r.register_count = prev->second->offset>>1 + prev->second->get_register_size();
r.register_count = total_register_count;
if (prev->second->register_type == ModbusRegisterType::COIL ||
prev->second->register_type == ModbusRegisterType::DISCRETE_INPUT) {
r.register_count = prev->second->offset + 1;
}
r.register_type = prev->second->register_type;
r.first_sensorkey = first_sensorkey;
r.skip_updates = skip_updates;
r.skip_updates_counter = 0;
ESP_LOGV(TAG, "Add last range 0x%X %d skip:%d", r.start_address, r.register_count, r.skip_updates); ESP_LOGV(TAG, "Add last range 0x%X %d skip:%d", r.start_address, r.register_count, r.skip_updates);
register_ranges_.push_back(r); register_ranges_.push_back(r);
} }
return register_ranges_.size(); return register_ranges_.size();
} }
@ -268,9 +276,15 @@ void ModbusController::dump_config() {
ESP_LOGCONFIG(TAG, " Address: 0x%02X", this->address_); ESP_LOGCONFIG(TAG, " Address: 0x%02X", this->address_);
#if ESPHOME_LOG_LEVEL >= ESPHOME_LOG_LEVEL_VERBOSE #if ESPHOME_LOG_LEVEL >= ESPHOME_LOG_LEVEL_VERBOSE
ESP_LOGCONFIG(TAG, "sensormap"); ESP_LOGCONFIG(TAG, "sensormap");
for (auto &it : sensormap_) { for (auto &it : sensorset_) {
ESP_LOGCONFIG("TAG", " Sensor 0x%llX start=0x%X count=%d size=%d", it.second->getkey(), it.second->start_address, ESP_LOGCONFIG(TAG, " Sensor type=%zu start=0x%X offset=0x%X count=%d size=%d",
it.second->register_count, it.second->get_register_size()); static_cast<uint8_t>(it->register_type), it->start_address, it->offset, it->register_count,
it->get_register_size());
}
ESP_LOGCONFIG(TAG, "ranges");
for (auto &it : register_ranges_) {
ESP_LOGCONFIG(TAG, " Range type=%zu start=0x%X count=%d skip_updates=%d", static_cast<uint8_t>(it.register_type),
it.start_address, it.register_count, it.skip_updates);
} }
#endif #endif
} }
@ -294,11 +308,11 @@ void ModbusController::on_write_register_response(ModbusRegisterType register_ty
ESP_LOGV(TAG, "Command ACK 0x%X %d ", get_data<uint16_t>(data, 0), get_data<int16_t>(data, 1)); ESP_LOGV(TAG, "Command ACK 0x%X %d ", get_data<uint16_t>(data, 0), get_data<int16_t>(data, 1));
} }
void ModbusController::dump_sensormap_() { void ModbusController::dump_sensors_() {
ESP_LOGV("modbuscontroller.h", "sensormap"); ESP_LOGV(TAG, "sensors");
for (auto &it : sensormap_) { for (auto &it : sensorset_) {
ESP_LOGV("modbuscontroller.h", " Sensor 0x%llX start=0x%X count=%d size=%d", it.second->getkey(), ESP_LOGV(TAG, " Sensor start=0x%X count=%d size=%d offset=%d", it->start_address, it->register_count,
it.second->start_address, it.second->register_count, it.second->get_register_size()); it->get_register_size(), it->offset);
} }
} }

79
esphome/components/modbus_controller/modbus_controller.h
View file

@ -6,7 +6,7 @@
#include "esphome/components/modbus/modbus.h" #include "esphome/components/modbus/modbus.h"
#include <list> #include <list>
#include <map> #include <set>
#include <queue> #include <queue>
#include <vector> #include <vector>
@ -37,7 +37,7 @@ enum class ModbusFunctionCode {
READ_FIFO_QUEUE = 0x18, // not implemented READ_FIFO_QUEUE = 0x18, // not implemented
}; };
enum class ModbusRegisterType : int { enum class ModbusRegisterType : uint8_t {
CUSTOM = 0x0, CUSTOM = 0x0,
COIL = 0x01, COIL = 0x01,
DISCRETE_INPUT = 0x02, DISCRETE_INPUT = 0x02,
@ -62,15 +62,6 @@ enum class SensorValueType : uint8_t {
FP32_R = 0xD FP32_R = 0xD
}; };
struct RegisterRange {
uint16_t start_address;
ModbusRegisterType register_type;
uint8_t register_count;
uint8_t skip_updates; // the config value
uint64_t first_sensorkey;
uint8_t skip_updates_counter; // the running value
} __attribute__((packed));
inline ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type) { inline ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type) {
switch (reg_type) { switch (reg_type) {
case ModbusRegisterType::COIL: case ModbusRegisterType::COIL:
@ -108,18 +99,6 @@ inline ModbusFunctionCode modbus_register_write_function(ModbusRegisterType reg_
} }
} }
/** All sensors are stored in a map
* to enable binary sensors for values encoded as bits in the same register the key of each sensor
* the key is a 64 bit integer that combines the register properties
* sensormap_ is sorted by this key. The key ensures the correct order when creating consequtive ranges
* Format: function_code (8 bit) | start address (16 bit)| offset (8bit)| bitmask (32 bit)
*/
inline uint64_t calc_key(ModbusRegisterType register_type, uint16_t start_address, uint8_t offset = 0,
uint32_t bitmask = 0) {
return uint64_t((uint16_t(register_type) << 24) + (uint32_t(start_address) << 8) + (offset & 0xFF)) << 32 | bitmask;
}
inline uint16_t register_from_key(uint64_t key) { return (key >> 40) & 0xFFFF; }
inline uint8_t c_to_hex(char c) { return (c >= 'A') ? (c >= 'a') ? (c - 'a' + 10) : (c - 'A' + 10) : (c - '0'); } inline uint8_t c_to_hex(char c) { return (c >= 'A') ? (c >= 'a') ? (c - 'a' + 10) : (c - 'A' + 10) : (c - '0'); }
/** Get a byte from a hex string /** Get a byte from a hex string
@ -250,7 +229,6 @@ class SensorItem {
virtual void parse_and_publish(const std::vector<uint8_t> &data) = 0; 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; } void set_custom_data(const std::vector<uint8_t> &data) { custom_data = data; }
uint64_t getkey() const { return calc_key(register_type, start_address, offset, bitmask); }
size_t virtual get_register_size() const { size_t virtual get_register_size() const {
if (register_type == ModbusRegisterType::COIL || register_type == ModbusRegisterType::DISCRETE_INPUT) if (register_type == ModbusRegisterType::COIL || register_type == ModbusRegisterType::DISCRETE_INPUT)
return 1; return 1;
@ -271,6 +249,48 @@ class SensorItem {
bool force_new_range{false}; bool force_new_range{false};
}; };
// 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;
uint8_t skip_updates; // the config value
SensorSet sensors; // all sensors of this range
uint8_t skip_updates_counter; // the running value
};
class ModbusCommandItem { class ModbusCommandItem {
public: public:
static const size_t MAX_PAYLOAD_BYTES = 240; static const size_t MAX_PAYLOAD_BYTES = 240;
@ -382,8 +402,8 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
/// queues a modbus command in the send queue /// queues a modbus command in the send queue
void queue_command(const ModbusCommandItem &command); void queue_command(const ModbusCommandItem &command);
/// Registers a sensor with the controller. Called by esphomes code generator /// Registers a sensor with the controller. Called by esphomes code generator
void add_sensor_item(SensorItem *item) { sensormap_[item->getkey()] = item; } void add_sensor_item(SensorItem *item) { sensorset_.insert(item); }
/// called when a modbus response was prased without errors /// called when a modbus response was parsed without errors
void on_modbus_data(const std::vector<uint8_t> &data) override; void on_modbus_data(const std::vector<uint8_t> &data) override;
/// called when a modbus error response was received /// called when a modbus error response was received
void on_modbus_error(uint8_t function_code, uint8_t exception_code) override; void on_modbus_error(uint8_t function_code, uint8_t exception_code) override;
@ -400,7 +420,7 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
/// parse sensormap_ and create range of sequential addresses /// parse sensormap_ and create range of sequential addresses
size_t create_register_ranges_(); size_t create_register_ranges_();
// find register in sensormap. Returns iterator with all registers having the same start address // find register in sensormap. Returns iterator with all registers having the same start address
std::map<uint64_t, SensorItem *>::iterator find_register_(ModbusRegisterType register_type, uint16_t start_address); SensorSet find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const;
/// submit the read command for the address range to the send queue /// submit the read command for the address range to the send queue
void update_range_(RegisterRange &r); void update_range_(RegisterRange &r);
/// parse incoming modbus data /// parse incoming modbus data
@ -410,10 +430,9 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
/// get the number of queued modbus commands (should be mostly empty) /// get the number of queued modbus commands (should be mostly empty)
size_t get_command_queue_length_() { return command_queue_.size(); } size_t get_command_queue_length_() { return command_queue_.size(); }
/// dump the parsed sensormap for diagnostics /// dump the parsed sensormap for diagnostics
void dump_sensormap_(); void dump_sensors_();
/// Collection of all sensors for this component /// Collection of all sensors for this component
/// see calc_key how the key is contructed SensorSet sensorset_;
std::map<uint64_t, SensorItem *> sensormap_;
/// Continous range of modbus registers /// Continous range of modbus registers
std::vector<RegisterRange> register_ranges_; std::vector<RegisterRange> register_ranges_;
/// Hold the pending requests to be sent /// Hold the pending requests to be sent