mirror of
https://github.com/esphome/esphome.git
synced 2024-11-22 15:08:10 +01:00
Fix register ranges in modbus controller (#2981)
This commit is contained in:
stegm
GitHub
parent
470071e0b0
commit
e4555f6997
2 changed files with 180 additions and 147 deletions
|
|
@ -24,17 +24,24 @@ bool ModbusController::send_next_command_() {
|
|||
if ((last_send > this->command_throttle_) && !waiting_for_response() && !command_queue_.empty()) {
|
||||
auto &command = command_queue_.front();
|
||||
|
||||
// remove from queue if command was sent too often
|
||||
if (command->send_countdown < 1) {
|
||||
ESP_LOGD(
|
||||
TAG,
|
||||
"Modbus command to device=%d register=0x%02X countdown=%d no response received - removed from send queue",
|
||||
this->address_, command->register_address, command->send_countdown);
|
||||
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 or command was sent too often
|
||||
if (!command->on_data_func || command->send_countdown < 1) {
|
||||
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);
|
||||
// remove from queue if no handler is defined
|
||||
if (!command->on_data_func) {
|
||||
command_queue_.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
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,
|
||||
uint16_t start_address) {
|
||||
auto vec_it = find_if(begin(register_ranges_), end(register_ranges_), [=](RegisterRange const &r) {
|
||||
SensorSet ModbusController::find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const {
|
||||
auto reg_it = find_if(begin(register_ranges_), end(register_ranges_), [=](RegisterRange const &r) {
|
||||
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);
|
||||
} else {
|
||||
auto map_it = sensormap_.find(vec_it->first_sensorkey);
|
||||
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);
|
||||
}
|
||||
return reg_it->sensors;
|
||||
}
|
||||
|
||||
// not found
|
||||
return std::end(sensormap_);
|
||||
return {};
|
||||
}
|
||||
void ModbusController::on_register_data(ModbusRegisterType register_type, uint16_t start_address,
|
||||
const std::vector<uint8_t> &data) {
|
||||
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
|
||||
while (map_it != sensormap_.end() && map_it->second->start_address == start_address) {
|
||||
if (map_it->second->register_type == register_type) {
|
||||
map_it->second->parse_and_publish(data);
|
||||
}
|
||||
map_it++;
|
||||
auto sensors = find_sensors_(register_type, start_address);
|
||||
for (auto sensor : sensors) {
|
||||
sensor->parse_and_publish(data);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -127,15 +126,16 @@ void ModbusController::update_range_(RegisterRange &r) {
|
|||
if (r.skip_updates_counter == 0) {
|
||||
// if a custom command is used the user supplied custom_data is only available in the SensorItem.
|
||||
if (r.register_type == ModbusRegisterType::CUSTOM) {
|
||||
auto it = this->find_register_(r.register_type, r.start_address);
|
||||
if (it != sensormap_.end()) {
|
||||
auto sensors = this->find_sensors_(r.register_type, r.start_address);
|
||||
if (!sensors.empty()) {
|
||||
auto sensor = sensors.cbegin();
|
||||
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->on_register_data(ModbusRegisterType::CUSTOM, start_address, data);
|
||||
});
|
||||
command_item.register_address = it->second->start_address;
|
||||
command_item.register_count = it->second->register_count;
|
||||
command_item.register_address = (*sensor)->start_address;
|
||||
command_item.register_count = (*sensor)->register_count;
|
||||
command_item.function_code = ModbusFunctionCode::CUSTOM;
|
||||
queue_command(command_item);
|
||||
}
|
||||
|
|
@ -167,99 +167,107 @@ void ModbusController::update() {
|
|||
// walk through the sensors and determine the register ranges to read
|
||||
size_t ModbusController::create_register_ranges_() {
|
||||
register_ranges_.clear();
|
||||
uint8_t n = 0;
|
||||
if (sensormap_.empty()) {
|
||||
if (sensorset_.empty()) {
|
||||
ESP_LOGW(TAG, "No sensors registered");
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto ix = sensormap_.begin();
|
||||
auto prev = ix;
|
||||
int total_register_count = 0;
|
||||
uint16_t current_start_address = ix->second->start_address;
|
||||
uint8_t buffer_offset = ix->second->offset;
|
||||
uint8_t skip_updates = ix->second->skip_updates;
|
||||
auto first_sensorkey = ix->second->getkey();
|
||||
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();
|
||||
// iterator is sorted see SensorItemsComparator for details
|
||||
auto ix = sensorset_.begin();
|
||||
RegisterRange r = {};
|
||||
uint8_t buffer_offset = 0;
|
||||
SensorItem *prev = nullptr;
|
||||
while (ix != sensorset_.end()) {
|
||||
SensorItem *curr = *ix;
|
||||
|
||||
// replace entry in sensormap_
|
||||
auto const value = ix->second;
|
||||
sensormap_.erase(ix);
|
||||
sensormap_.insert({value->getkey(), value});
|
||||
// move iterator back to new element
|
||||
ix = sensormap_.find(value->getkey()); // next(prev, 1);
|
||||
}
|
||||
if (current_start_address != ix->second->start_address ||
|
||||
// ( prev->second->start_address + prev->second->offset != ix->second->start_address) ||
|
||||
ix->second->register_type != prev->second->register_type) {
|
||||
// Difference doesn't match so we have a gap
|
||||
if (n > 0) {
|
||||
RegisterRange r;
|
||||
r.start_address = current_start_address;
|
||||
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;
|
||||
ESP_LOGV(TAG, "Register: 0x%X %d %d %d offset=%u skip=%u addr=%p", curr->start_address, curr->register_count,
|
||||
curr->offset, curr->get_register_size(), curr->offset, curr->skip_updates, curr);
|
||||
|
||||
if (r.register_count == 0) {
|
||||
// this is the first register in range
|
||||
r.start_address = curr->start_address;
|
||||
r.register_count = curr->register_count;
|
||||
r.register_type = curr->register_type;
|
||||
r.sensors.insert(curr);
|
||||
r.skip_updates = curr->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;
|
||||
buffer_offset = curr->get_register_size();
|
||||
|
||||
ESP_LOGV(TAG, "Started new range");
|
||||
} else {
|
||||
n++;
|
||||
if (ix->second->offset != prev->second->offset || n == 1) {
|
||||
total_register_count += ix->second->register_count;
|
||||
buffer_offset += ix->second->get_register_size();
|
||||
// this is not the first register in range so it might be possible
|
||||
// to reuse the last register or extend the current range
|
||||
if (!curr->force_new_range && r.register_type == curr->register_type &&
|
||||
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
|
||||
// Because zero is the default value for skip_updates it is excluded from getting the min value.
|
||||
if (ix->second->skip_updates != 0) {
|
||||
if (skip_updates != 0) {
|
||||
skip_updates = std::min(skip_updates, ix->second->skip_updates);
|
||||
if (curr->skip_updates != 0) {
|
||||
if (r.skip_updates != 0) {
|
||||
r.skip_updates = std::min(r.skip_updates, curr->skip_updates);
|
||||
} 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;
|
||||
}
|
||||
|
||||
if (r.register_count > 0) {
|
||||
// Add the last range
|
||||
if (n > 0) {
|
||||
RegisterRange r;
|
||||
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);
|
||||
register_ranges_.push_back(r);
|
||||
}
|
||||
|
||||
return register_ranges_.size();
|
||||
}
|
||||
|
||||
|
|
@ -268,9 +276,15 @@ void ModbusController::dump_config() {
|
|||
ESP_LOGCONFIG(TAG, " Address: 0x%02X", this->address_);
|
||||
#if ESPHOME_LOG_LEVEL >= ESPHOME_LOG_LEVEL_VERBOSE
|
||||
ESP_LOGCONFIG(TAG, "sensormap");
|
||||
for (auto &it : sensormap_) {
|
||||
ESP_LOGCONFIG("TAG", " Sensor 0x%llX start=0x%X count=%d size=%d", it.second->getkey(), it.second->start_address,
|
||||
it.second->register_count, it.second->get_register_size());
|
||||
for (auto &it : sensorset_) {
|
||||
ESP_LOGCONFIG(TAG, " Sensor type=%zu start=0x%X offset=0x%X count=%d size=%d",
|
||||
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
|
||||
}
|
||||
|
|
@ -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));
|
||||
}
|
||||
|
||||
void ModbusController::dump_sensormap_() {
|
||||
ESP_LOGV("modbuscontroller.h", "sensormap");
|
||||
for (auto &it : sensormap_) {
|
||||
ESP_LOGV("modbuscontroller.h", " Sensor 0x%llX start=0x%X count=%d size=%d", it.second->getkey(),
|
||||
it.second->start_address, it.second->register_count, it.second->get_register_size());
|
||||
void ModbusController::dump_sensors_() {
|
||||
ESP_LOGV(TAG, "sensors");
|
||||
for (auto &it : sensorset_) {
|
||||
ESP_LOGV(TAG, " Sensor start=0x%X count=%d size=%d offset=%d", it->start_address, it->register_count,
|
||||
it->get_register_size(), it->offset);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include "esphome/components/modbus/modbus.h"
|
||||
|
||||
#include <list>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <queue>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -37,7 +37,7 @@ enum class ModbusFunctionCode {
|
|||
READ_FIFO_QUEUE = 0x18, // not implemented
|
||||
};
|
||||
|
||||
enum class ModbusRegisterType : int {
|
||||
enum class ModbusRegisterType : uint8_t {
|
||||
CUSTOM = 0x0,
|
||||
COIL = 0x01,
|
||||
DISCRETE_INPUT = 0x02,
|
||||
|
|
@ -62,15 +62,6 @@ enum class SensorValueType : uint8_t {
|
|||
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) {
|
||||
switch (reg_type) {
|
||||
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'); }
|
||||
|
||||
/** 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;
|
||||
|
||||
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 {
|
||||
if (register_type == ModbusRegisterType::COIL || register_type == ModbusRegisterType::DISCRETE_INPUT)
|
||||
return 1;
|
||||
|
|
@ -271,6 +249,48 @@ class SensorItem {
|
|||
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 {
|
||||
public:
|
||||
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
|
||||
void queue_command(const ModbusCommandItem &command);
|
||||
/// Registers a sensor with the controller. Called by esphomes code generator
|
||||
void add_sensor_item(SensorItem *item) { sensormap_[item->getkey()] = item; }
|
||||
/// called when a modbus response was prased without errors
|
||||
void add_sensor_item(SensorItem *item) { sensorset_.insert(item); }
|
||||
/// called when a modbus response was parsed without errors
|
||||
void on_modbus_data(const std::vector<uint8_t> &data) override;
|
||||
/// called when a modbus error response was received
|
||||
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
|
||||
size_t create_register_ranges_();
|
||||
// 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
|
||||
void update_range_(RegisterRange &r);
|
||||
/// 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)
|
||||
size_t get_command_queue_length_() { return command_queue_.size(); }
|
||||
/// dump the parsed sensormap for diagnostics
|
||||
void dump_sensormap_();
|
||||
void dump_sensors_();
|
||||
/// Collection of all sensors for this component
|
||||
/// see calc_key how the key is contructed
|
||||
std::map<uint64_t, SensorItem *> sensormap_;
|
||||
SensorSet sensorset_;
|
||||
/// Continous range of modbus registers
|
||||
std::vector<RegisterRange> register_ranges_;
|
||||
/// Hold the pending requests to be sent
|
||||
|
|
|
|||
Loading…
Reference in a new issue