diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp index 8b96c20691..2fdedfd786 100644 --- a/esphome/components/modbus_controller/modbus_controller.cpp +++ b/esphome/components/modbus_controller/modbus_controller.cpp @@ -24,15 +24,22 @@ bool ModbusController::send_next_command_() { if ((last_send > this->command_throttle_) && !waiting_for_response() && !command_queue_.empty()) { auto &command = command_queue_.front(); - 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 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 + 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::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()) { - ESP_LOGE(TAG, "No matching range for sensor found - start_address : 0x%X", start_address); + 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 &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 &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); } @@ -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_() { 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; - 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; + 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; + 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; } - // 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; + + if (r.register_count > 0) { + // Add the last range 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(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(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(data, 0), get_data(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); } } diff --git a/esphome/components/modbus_controller/modbus_controller.h b/esphome/components/modbus_controller/modbus_controller.h index f4948e6ff9..6dbabac71e 100644 --- a/esphome/components/modbus_controller/modbus_controller.h +++ b/esphome/components/modbus_controller/modbus_controller.h @@ -6,7 +6,7 @@ #include "esphome/components/modbus/modbus.h" #include -#include +#include #include #include @@ -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 &data) = 0; void set_custom_data(const std::vector &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; + +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 &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::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 sensormap_; + SensorSet sensorset_; /// Continous range of modbus registers std::vector register_ranges_; /// Hold the pending requests to be sent