[modbus] Share helper functions across modbus components - part B (#14172)

Co-authored-by: J. Nick Koston <nick+github@koston.org>
Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
This commit is contained in:
Bonne Eggleston
2026-03-31 13:48:16 -07:00
committed by GitHub
parent da6c4e20fe
commit 2cb987095d
9 changed files with 277 additions and 222 deletions
@@ -0,0 +1,139 @@
#include "modbus_helpers.h"
#include "esphome/core/log.h"
namespace esphome::modbus::helpers {
static const char *const TAG = "modbus_helpers";
void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type) {
switch (value_type) {
case SensorValueType::U_WORD:
case SensorValueType::S_WORD:
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_DWORD:
case SensorValueType::S_DWORD:
case SensorValueType::FP32:
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_DWORD_R:
case SensorValueType::S_DWORD_R:
case SensorValueType::FP32_R:
data.push_back(value & 0xFFFF);
data.push_back((value & 0xFFFF0000) >> 16);
break;
case SensorValueType::U_QWORD:
case SensorValueType::S_QWORD:
data.push_back((value & 0xFFFF000000000000) >> 48);
data.push_back((value & 0xFFFF00000000) >> 32);
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_QWORD_R:
case SensorValueType::S_QWORD_R:
data.push_back(value & 0xFFFF);
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back((value & 0xFFFF00000000) >> 32);
data.push_back((value & 0xFFFF000000000000) >> 48);
break;
default:
ESP_LOGE(TAG, "Invalid data type for modbus number to payload conversion: %d", static_cast<uint16_t>(value_type));
break;
}
}
int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
uint32_t bitmask) {
int64_t value = 0; // int64_t because it can hold signed and unsigned 32 bits
if (offset > data.size()) {
ESP_LOGE(TAG, "not enough data for value");
return value;
}
size_t size = data.size() - offset;
bool error = false;
switch (sensor_value_type) {
case SensorValueType::U_WORD:
if (size >= 2) {
value = mask_and_shift_by_rightbit(get_data<uint16_t>(data, offset),
bitmask); // default is 0xFFFF ;
} else {
error = true;
}
break;
case SensorValueType::U_DWORD:
case SensorValueType::FP32:
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
} else {
error = true;
}
break;
case SensorValueType::U_DWORD_R:
case SensorValueType::FP32_R:
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
value = static_cast<uint32_t>(value & 0xFFFF) << 16 | (value & 0xFFFF0000) >> 16;
value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
} else {
error = true;
}
break;
case SensorValueType::S_WORD:
if (size >= 2) {
value = mask_and_shift_by_rightbit(get_data<int16_t>(data, offset),
bitmask); // default is 0xFFFF ;
} else {
error = true;
}
break;
case SensorValueType::S_DWORD:
if (size >= 4) {
value = mask_and_shift_by_rightbit(get_data<int32_t>(data, offset), bitmask);
} else {
error = true;
}
break;
case SensorValueType::S_DWORD_R: {
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
// Currently the high word is at the low position
// the sign bit is therefore at low before the switch
uint32_t sign_bit = (value & 0x8000) << 16;
value = mask_and_shift_by_rightbit(
static_cast<int32_t>(((value & 0x7FFF) << 16 | (value & 0xFFFF0000) >> 16) | sign_bit), bitmask);
} else {
error = true;
}
} break;
case SensorValueType::U_QWORD:
case SensorValueType::S_QWORD:
// Ignore bitmask for QWORD
if (size >= 8) {
value = get_data<uint64_t>(data, offset);
} else {
error = true;
}
break;
case SensorValueType::U_QWORD_R:
case SensorValueType::S_QWORD_R: {
// Ignore bitmask for QWORD
if (size >= 8) {
uint64_t tmp = get_data<uint64_t>(data, offset);
value = (tmp << 48) | (tmp >> 48) | ((tmp & 0xFFFF0000) << 16) | ((tmp >> 16) & 0xFFFF0000);
} else {
error = true;
}
} break;
case SensorValueType::RAW:
default:
break;
}
if (error)
ESP_LOGE(TAG, "not enough data for value");
return value;
}
} // namespace esphome::modbus::helpers
+101
View File
@@ -1,6 +1,8 @@
#pragma once
#include <string>
#include <vector>
#include <cmath>
#include "esphome/core/helpers.h"
#include "esphome/components/modbus/modbus_definitions.h"
@@ -103,4 +105,103 @@ inline uint64_t qword_from_hex_str(const std::string &value, uint8_t pos) {
return static_cast<uint64_t>(dword_from_hex_str(value, pos)) << 32 | dword_from_hex_str(value, pos + 4);
}
// Extract data from modbus response buffer
/** Extract data from modbus response buffer
* @param T one of supported integer data types int_8,int_16,int_32,int_64
* @param data modbus response buffer (uint8_t)
* @param buffer_offset offset in bytes.
* @return value of type T extracted from buffer
*/
template<typename T> T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
if (sizeof(T) == sizeof(uint8_t)) {
return T(data[buffer_offset]);
}
if (sizeof(T) == sizeof(uint16_t)) {
return T((uint16_t(data[buffer_offset + 0]) << 8) | (uint16_t(data[buffer_offset + 1]) << 0));
}
if (sizeof(T) == sizeof(uint32_t)) {
return static_cast<uint32_t>(get_data<uint16_t>(data, buffer_offset)) << 16 |
static_cast<uint32_t>(get_data<uint16_t>(data, buffer_offset + 2));
}
if (sizeof(T) == sizeof(uint64_t)) {
return static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset)) << 32 |
(static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset + 4)));
}
static_assert(sizeof(T) == sizeof(uint8_t) || sizeof(T) == sizeof(uint16_t) || sizeof(T) == sizeof(uint32_t) ||
sizeof(T) == sizeof(uint64_t),
"Unsupported type size in get_data; only 1, 2, 4, or 8-byte integer types are supported.");
return T{};
}
/** Extract coil data from modbus response buffer
* Responses for coil are packed into bytes .
* coil 3 is bit 3 of the first response byte
* coil 9 is bit 2 of the second response byte
* @param coil number of the cil
* @param data modbus response buffer (uint8_t)
* @return content of coil register
*/
inline bool coil_from_vector(int coil, const std::vector<uint8_t> &data) {
auto data_byte = coil / 8;
return (data[data_byte] & (1 << (coil % 8))) > 0;
}
/** Extract bits from value and shift right according to the bitmask
* if the bitmask is 0x00F0 we want the values frrom bit 5 - 8.
* the result is then shifted right by the position if the first right set bit in the mask
* Useful for modbus data where more than one value is packed in a 16 bit register
* Example: on Epever the "Length of night" register 0x9065 encodes values of the whole night length of time as
* D15 - D8 = hour, D7 - D0 = minute
* To get the hours use mask 0xFF00 and 0x00FF for the minute
* @param data an integral value between 16 aand 32 bits,
* @param bitmask the bitmask to apply
*/
template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
auto result = (mask & data);
if (result == 0 || mask == 0xFFFFFFFF) {
return result;
}
for (size_t pos = 0; pos < sizeof(N) << 3; pos++) {
if (pos < 32 && (mask & (1UL << pos)) != 0)
return result >> pos;
}
return 0;
}
/** Convert float value to vector<uint16_t> suitable for sending
* @param data target for payload
* @param value float value to convert
* @param value_type defines if 16/32 or FP32 is used
* @return vector containing the modbus register words in correct order
*/
void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type);
/** Convert vector<uint8_t> response payload to number.
* @param data payload with the data to convert
* @param sensor_value_type defines if 16/32/64 bits or FP32 is used
* @param offset offset to the data in data
* @param bitmask bitmask used for masking and shifting
* @return 64-bit number of the payload
*/
int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
uint32_t bitmask);
inline std::vector<uint16_t> float_to_payload(float value, SensorValueType value_type) {
int64_t val;
if (value_type_is_float(value_type)) {
val = bit_cast<uint32_t>(value);
} else {
val = llroundf(value);
}
std::vector<uint16_t> data;
number_to_payload(data, val, value_type);
return data;
}
} // namespace esphome::modbus::helpers
@@ -15,10 +15,10 @@ void ModbusBinarySensor::parse_and_publish(const std::vector<uint8_t> &data) {
case ModbusRegisterType::DISCRETE_INPUT:
case ModbusRegisterType::COIL:
// offset for coil is the actual number of the coil not the byte offset
value = coil_from_vector(this->offset, data);
value = modbus::helpers::coil_from_vector(this->offset, data);
break;
default:
value = get_data<uint16_t>(data, this->offset) & this->bitmask;
value = modbus::helpers::get_data<uint16_t>(data, this->offset) & this->bitmask;
break;
}
// Is there a lambda registered
@@ -140,7 +140,7 @@ void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t
std::vector<uint16_t> payload;
payload.reserve(server_register->register_count * 2);
number_to_payload(payload, value, server_register->value_type);
modbus::helpers::number_to_payload(payload, value, server_register->value_type);
sixteen_bit_response.insert(sixteen_bit_response.end(), payload.cbegin(), payload.cend());
current_address += server_register->register_count;
found = true;
@@ -258,7 +258,7 @@ void ModbusController::on_modbus_write_registers(uint8_t function_code, const st
// Actually write to the registers:
if (!for_each_register([&data](ServerRegister *server_register, uint16_t offset) {
int64_t number = payload_to_number(data, server_register->value_type, offset, 0xFFFFFFFF);
int64_t number = modbus::helpers::payload_to_number(data, server_register->value_type, offset, 0xFFFFFFFF);
return server_register->write_lambda(number);
})) {
this->send_error(function_code, ModbusExceptionCode::SERVICE_DEVICE_FAILURE);
@@ -517,7 +517,8 @@ void ModbusController::loop() {
void ModbusController::on_write_register_response(ModbusRegisterType register_type, uint16_t start_address,
const std::vector<uint8_t> &data) {
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 ", modbus::helpers::get_data<uint16_t>(data, 0),
modbus::helpers::get_data<int16_t>(data, 1));
}
void ModbusController::dump_sensors_() {
@@ -710,132 +711,5 @@ bool ModbusCommandItem::is_equal(const ModbusCommandItem &other) {
other.register_type == this->register_type && other.function_code == this->function_code;
}
void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type) {
switch (value_type) {
case SensorValueType::U_WORD:
case SensorValueType::S_WORD:
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_DWORD:
case SensorValueType::S_DWORD:
case SensorValueType::FP32:
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_DWORD_R:
case SensorValueType::S_DWORD_R:
case SensorValueType::FP32_R:
data.push_back(value & 0xFFFF);
data.push_back((value & 0xFFFF0000) >> 16);
break;
case SensorValueType::U_QWORD:
case SensorValueType::S_QWORD:
data.push_back((value & 0xFFFF000000000000) >> 48);
data.push_back((value & 0xFFFF00000000) >> 32);
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back(value & 0xFFFF);
break;
case SensorValueType::U_QWORD_R:
case SensorValueType::S_QWORD_R:
data.push_back(value & 0xFFFF);
data.push_back((value & 0xFFFF0000) >> 16);
data.push_back((value & 0xFFFF00000000) >> 32);
data.push_back((value & 0xFFFF000000000000) >> 48);
break;
default:
ESP_LOGE(TAG, "Invalid data type for modbus number to payload conversation: %d",
static_cast<uint16_t>(value_type));
break;
}
}
int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
uint32_t bitmask) {
int64_t value = 0; // int64_t because it can hold signed and unsigned 32 bits
size_t size = data.size() - offset;
bool error = false;
switch (sensor_value_type) {
case SensorValueType::U_WORD:
if (size >= 2) {
value = mask_and_shift_by_rightbit(get_data<uint16_t>(data, offset), bitmask); // default is 0xFFFF ;
} else {
error = true;
}
break;
case SensorValueType::U_DWORD:
case SensorValueType::FP32:
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
} else {
error = true;
}
break;
case SensorValueType::U_DWORD_R:
case SensorValueType::FP32_R:
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
value = static_cast<uint32_t>(value & 0xFFFF) << 16 | (value & 0xFFFF0000) >> 16;
value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
} else {
error = true;
}
break;
case SensorValueType::S_WORD:
if (size >= 2) {
value = mask_and_shift_by_rightbit(get_data<int16_t>(data, offset),
bitmask); // default is 0xFFFF ;
} else {
error = true;
}
break;
case SensorValueType::S_DWORD:
if (size >= 4) {
value = mask_and_shift_by_rightbit(get_data<int32_t>(data, offset), bitmask);
} else {
error = true;
}
break;
case SensorValueType::S_DWORD_R: {
if (size >= 4) {
value = get_data<uint32_t>(data, offset);
// Currently the high word is at the low position
// the sign bit is therefore at low before the switch
uint32_t sign_bit = (value & 0x8000) << 16;
value = mask_and_shift_by_rightbit(
static_cast<int32_t>(((value & 0x7FFF) << 16 | (value & 0xFFFF0000) >> 16) | sign_bit), bitmask);
} else {
error = true;
}
} break;
case SensorValueType::U_QWORD:
case SensorValueType::S_QWORD:
// Ignore bitmask for QWORD
if (size >= 8) {
value = get_data<uint64_t>(data, offset);
} else {
error = true;
}
break;
case SensorValueType::U_QWORD_R:
case SensorValueType::S_QWORD_R: {
// Ignore bitmask for QWORD
if (size >= 8) {
uint64_t tmp = get_data<uint64_t>(data, offset);
value = (tmp << 48) | (tmp >> 48) | ((tmp & 0xFFFF0000) << 16) | ((tmp >> 16) & 0xFFFF0000);
} else {
error = true;
}
} break;
case SensorValueType::RAW:
default:
break;
}
if (error)
ESP_LOGE(TAG, "not enough data for value");
return value;
}
} // namespace modbus_controller
} // namespace esphome
@@ -59,83 +59,38 @@ inline uint64_t qword_from_hex_str(const std::string &value, uint8_t pos) {
return modbus::helpers::qword_from_hex_str(value, pos);
}
// Extract data from modbus response buffer
/** Extract data from modbus response buffer
* @param T one of supported integer data types int_8,int_16,int_32,int_64
* @param data modbus response buffer (uint8_t)
* @param buffer_offset offset in bytes.
* @return value of type T extracted from buffer
*/
template<typename T> T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
if (sizeof(T) == sizeof(uint8_t)) {
return T(data[buffer_offset]);
}
if (sizeof(T) == sizeof(uint16_t)) {
return T((uint16_t(data[buffer_offset + 0]) << 8) | (uint16_t(data[buffer_offset + 1]) << 0));
}
if (sizeof(T) == sizeof(uint32_t)) {
return get_data<uint16_t>(data, buffer_offset) << 16 | get_data<uint16_t>(data, (buffer_offset + 2));
}
if (sizeof(T) == sizeof(uint64_t)) {
return static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset)) << 32 |
(static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset + 4)));
}
template<typename T>
ESPDEPRECATED("Use modbus::helpers::get_data() instead. Removed in 2026.10.0", "2026.4.0")
T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
return modbus::helpers::get_data<T>(data, buffer_offset);
}
/** Extract coil data from modbus response buffer
* Responses for coil are packed into bytes .
* coil 3 is bit 3 of the first response byte
* coil 9 is bit 2 of the second response byte
* @param coil number of the cil
* @param data modbus response buffer (uint8_t)
* @return content of coil register
*/
ESPDEPRECATED("Use modbus::helpers::coil_from_vector() instead. Removed in 2026.10.0", "2026.4.0")
inline bool coil_from_vector(int coil, const std::vector<uint8_t> &data) {
auto data_byte = coil / 8;
return (data[data_byte] & (1 << (coil % 8))) > 0;
return modbus::helpers::coil_from_vector(coil, data);
}
/** Extract bits from value and shift right according to the bitmask
* if the bitmask is 0x00F0 we want the values frrom bit 5 - 8.
* the result is then shifted right by the position if the first right set bit in the mask
* Useful for modbus data where more than one value is packed in a 16 bit register
* Example: on Epever the "Length of night" register 0x9065 encodes values of the whole night length of time as
* D15 - D8 = hour, D7 - D0 = minute
* To get the hours use mask 0xFF00 and 0x00FF for the minute
* @param data an integral value between 16 aand 32 bits,
* @param bitmask the bitmask to apply
*/
template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
auto result = (mask & data);
if (result == 0 || mask == 0xFFFFFFFF) {
return result;
}
for (size_t pos = 0; pos < sizeof(N) << 3; pos++) {
if ((mask & (1UL << pos)) != 0)
return result >> pos;
}
return 0;
template<typename N>
ESPDEPRECATED("Use modbus::helpers::mask_and_shift_by_rightbit() instead. Removed in 2026.10.0", "2026.4.0")
N mask_and_shift_by_rightbit(N data, uint32_t mask) {
return modbus::helpers::mask_and_shift_by_rightbit(data, mask);
}
/** Convert float value to vector<uint16_t> suitable for sending
* @param data target for payload
* @param value float value to convert
* @param value_type defines if 16/32 or FP32 is used
* @return vector containing the modbus register words in correct order
*/
void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type);
ESPDEPRECATED("Use modbus::helpers::number_to_payload() instead. Removed in 2026.10.0", "2026.4.0")
inline void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type) {
modbus::helpers::number_to_payload(data, value, value_type);
}
/** Convert vector<uint8_t> response payload to number.
* @param data payload with the data to convert
* @param sensor_value_type defines if 16/32/64 bits or FP32 is used
* @param offset offset to the data in data
* @param bitmask bitmask used for masking and shifting
* @return 64-bit number of the payload
*/
int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
uint32_t bitmask);
ESPDEPRECATED("Use modbus::helpers::payload_to_number() instead. Removed in 2026.10.0", "2026.4.0")
inline int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
uint32_t bitmask) {
return modbus::helpers::payload_to_number(data, sensor_value_type, offset, bitmask);
}
ESPDEPRECATED("Use modbus::helpers::float_to_payload() instead. Removed in 2026.10.0", "2026.4.0")
inline std::vector<uint16_t> float_to_payload(float value, SensorValueType value_type) {
return modbus::helpers::float_to_payload(value, value_type);
}
class ModbusController;
@@ -517,7 +472,7 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
* @return float value of data
*/
inline float payload_to_float(const std::vector<uint8_t> &data, const SensorItem &item) {
int64_t number = payload_to_number(data, item.sensor_value_type, item.offset, item.bitmask);
int64_t number = modbus::helpers::payload_to_number(data, item.sensor_value_type, item.offset, item.bitmask);
float float_value;
if (modbus::helpers::value_type_is_float(item.sensor_value_type)) {
@@ -529,19 +484,5 @@ inline float payload_to_float(const std::vector<uint8_t> &data, const SensorItem
return float_value;
}
inline std::vector<uint16_t> float_to_payload(float value, SensorValueType value_type) {
int64_t val;
if (modbus::helpers::value_type_is_float(value_type)) {
val = bit_cast<uint32_t>(value);
} else {
val = llroundf(value);
}
std::vector<uint16_t> data;
number_to_payload(data, val, value_type);
return data;
}
} // namespace modbus_controller
} // namespace esphome
@@ -62,7 +62,7 @@ void ModbusNumber::control(float value) {
this->parent_->on_write_register_response(write_cmd.register_type, this->start_address, data);
});
} else {
data = float_to_payload(write_value, this->sensor_value_type);
data = modbus::helpers::float_to_payload(write_value, this->sensor_value_type);
ESP_LOGD(TAG,
"Updating register: connected Sensor=%s start address=0x%X register count=%d new value=%.02f (val=%.02f)",
@@ -34,7 +34,7 @@ void ModbusFloatOutput::write_state(float value) {
}
// lambda didn't set payload
if (data.empty()) {
data = float_to_payload(value, this->sensor_value_type);
data = modbus::helpers::float_to_payload(value, this->sensor_value_type);
}
ESP_LOGD(TAG, "Updating register: start address=0x%X register count=%d new value=%.02f (val=%.02f)",
@@ -9,7 +9,7 @@ static const char *const TAG = "modbus_controller.select";
void ModbusSelect::dump_config() { LOG_SELECT(TAG, "Modbus Controller Select", this); }
void ModbusSelect::parse_and_publish(const std::vector<uint8_t> &data) {
int64_t value = payload_to_number(data, this->sensor_value_type, this->offset, this->bitmask);
int64_t value = modbus::helpers::payload_to_number(data, this->sensor_value_type, this->offset, this->bitmask);
ESP_LOGD(TAG, "New select value %lld from payload", value);
@@ -61,7 +61,7 @@ void ModbusSelect::control(size_t index) {
}
if (data.empty()) {
number_to_payload(data, *mapval, this->sensor_value_type);
modbus::helpers::number_to_payload(data, *mapval, this->sensor_value_type);
} else {
ESP_LOGV(TAG, "Using payload from write lambda");
}
@@ -33,10 +33,10 @@ void ModbusSwitch::parse_and_publish(const std::vector<uint8_t> &data) {
case ModbusRegisterType::DISCRETE_INPUT:
case ModbusRegisterType::COIL:
// offset for coil is the actual number of the coil not the byte offset
value = coil_from_vector(this->offset, data);
value = modbus::helpers::coil_from_vector(this->offset, data);
break;
default:
value = get_data<uint16_t>(data, this->offset) & this->bitmask;
value = modbus::helpers::get_data<uint16_t>(data, this->offset) & this->bitmask;
break;
}