sascha/esphome
1
0
Fork 0
mirror of https://github.com/esphome/esphome.git synced 2025-10-31 15:12:06 +00:00
Code Issues Releases Wiki Activity

[modbus] Modbus server role: write holding registers (#9156)

Browse Source
This commit is contained in:
Javier Peletier
2025-07-01 05:39:06 +02:00
committed by GitHub
parent 16ef5a9377
commit 35de36d690
6 changed files with 154 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
esphome/components/modbus/modbus.cpp
View File

@@ -90,15 +90,24 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
} else {
// data starts at 2 and length is 4 for read registers commands
if (this->role == ModbusRole::SERVER && (function_code == 0x1 || function_code == 0x3 || function_code == 0x4)) {
data_offset = 2;
data_len = 4;
}
// the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands
if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) {
data_offset = 2;
data_len = 4;
if (this->role == ModbusRole::SERVER) {
if (function_code == 0x1 || function_code == 0x3 || function_code == 0x4 || function_code == 0x6) {
data_offset = 2;
data_len = 4;
} else if (function_code == 0x10) {
if (at < 6) {
return true;
}
data_offset = 2;
// starting address (2 bytes) + quantity of registers (2 bytes) + byte count itself (1 byte) + actual byte count
data_len = 2 + 2 + 1 + raw[6];
}
} else {
// the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands
if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) {
data_offset = 2;
data_len = 4;
}
}
// Error ( msb indicates error )
@@ -132,6 +141,7 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
bool found = false;
for (auto *device : this->devices_) {
if (device->address_ == address) {
found = true;
// Is it an error response?
if ((function_code & 0x80) == 0x80) {
ESP_LOGD(TAG, "Modbus error function code: 0x%X exception: %d", function_code, raw[2]);
@@ -141,13 +151,21 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
// Ignore modbus exception not related to a pending command
ESP_LOGD(TAG, "Ignoring Modbus error - not expecting a response");
}
} else if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) {
device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8),
uint16_t(data[3]) | (uint16_t(data[2]) << 8));
} else {
device->on_modbus_data(data);
continue;
}
found = true;
if (this->role == ModbusRole::SERVER) {
if (function_code == 0x3 || function_code == 0x4) {
device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8),
uint16_t(data[3]) | (uint16_t(data[2]) << 8));
continue;
}
if (function_code == 0x6 || function_code == 0x10) {
device->on_modbus_write_registers(function_code, data);
continue;
}
}
// fallthrough for other function codes
device->on_modbus_data(data);
}
}
waiting_for_response = 0;

1
esphome/components/modbus/modbus.h
View File

@@ -59,6 +59,7 @@ class ModbusDevice {
virtual void on_modbus_data(const std::vector<uint8_t> &data) = 0;
virtual void on_modbus_error(uint8_t function_code, uint8_t exception_code) {}
virtual void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers){};
virtual void on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data){};
void send(uint8_t function, uint16_t start_address, uint16_t number_of_entities, uint8_t payload_len = 0,
const uint8_t *payload = nullptr) {
this->parent_->send(this->address_, function, start_address, number_of_entities, payload_len, payload);

13
esphome/components/modbus_controller/__init__.py
View File

@@ -39,6 +39,7 @@ CODEOWNERS = ["@martgras"]
AUTO_LOAD = ["modbus"]
CONF_READ_LAMBDA = "read_lambda"
CONF_WRITE_LAMBDA = "write_lambda"
CONF_SERVER_REGISTERS = "server_registers"
MULTI_CONF = True
@@ -148,6 +149,7 @@ ModbusServerRegisterSchema = cv.Schema(
cv.Required(CONF_ADDRESS): cv.positive_int,
cv.Optional(CONF_VALUE_TYPE, default="U_WORD"): cv.enum(SENSOR_VALUE_TYPE),
cv.Required(CONF_READ_LAMBDA): cv.returning_lambda,
cv.Optional(CONF_WRITE_LAMBDA): cv.returning_lambda,
}
)
@@ -318,6 +320,17 @@ async def to_code(config):
),
)
)
if CONF_WRITE_LAMBDA in server_register:
cg.add(
server_register_var.set_write_lambda(
cg.TemplateArguments(cpp_type),
await cg.process_lambda(
server_register[CONF_WRITE_LAMBDA],
parameters=[(cg.uint16, "address"), (cpp_type, "x")],
return_type=cg.bool_,
),
)
)
cg.add(var.add_server_register(server_register_var))
await register_modbus_device(var, config)
for conf in config.get(CONF_ON_COMMAND_SENT, []):

80
esphome/components/modbus_controller/modbus_controller.cpp
View File

@@ -152,6 +152,86 @@ void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t
this->send(function_code, start_address, number_of_registers, response.size(), response.data());
}
void ModbusController::on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data) {
uint16_t number_of_registers;
uint16_t payload_offset;
if (function_code == 0x10) {
number_of_registers = uint16_t(data[3]) | (uint16_t(data[2]) << 8);
if (number_of_registers == 0 || number_of_registers > 0x7B) {
ESP_LOGW(TAG, "Invalid number of registers %d. Sending exception response.", number_of_registers);
send_error(function_code, 3);
return;
}
uint16_t payload_size = data[4];
if (payload_size != number_of_registers * 2) {
ESP_LOGW(TAG, "Payload size of %d bytes is not 2 times the number of registers (%d). Sending exception response.",
payload_size, number_of_registers);
send_error(function_code, 3);
return;
}
payload_offset = 5;
} else if (function_code == 0x06) {
number_of_registers = 1;
payload_offset = 2;
} else {
ESP_LOGW(TAG, "Invalid function code 0x%X. Sending exception response.", function_code);
send_error(function_code, 1);
return;
}
uint16_t start_address = uint16_t(data[1]) | (uint16_t(data[0]) << 8);
ESP_LOGD(TAG,
"Received write holding registers for device 0x%X. FC: 0x%X. Start address: 0x%X. Number of registers: "
"0x%X.",
this->address_, function_code, start_address, number_of_registers);
auto for_each_register = [this, start_address, number_of_registers, payload_offset](
const std::function<bool(ServerRegister *, uint16_t offset)> &callback) -> bool {
uint16_t offset = payload_offset;
for (uint16_t current_address = start_address; current_address < start_address + number_of_registers;) {
bool ok = false;
for (auto *server_register : this->server_registers_) {
if (server_register->address == current_address) {
ok = callback(server_register, offset);
current_address += server_register->register_count;
offset += server_register->register_count * sizeof(uint16_t);
break;
}
}
if (!ok) {
return false;
}
}
return true;
};
// check all registers are writable before writing to any of them:
if (!for_each_register([](ServerRegister *server_register, uint16_t offset) -> bool {
return server_register->write_lambda != nullptr;
})) {
send_error(function_code, 1);
return;
}
// 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);
return server_register->write_lambda(number);
})) {
send_error(function_code, 4);
return;
}
std::vector<uint8_t> response;
response.reserve(6);
response.push_back(this->address_);
response.push_back(function_code);
response.insert(response.end(), data.begin(), data.begin() + 4);
this->send_raw(response);
}
SensorSet ModbusController::find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const {
auto reg_it = std::find_if(
std::begin(this->register_ranges_), std::end(this->register_ranges_),

15
esphome/components/modbus_controller/modbus_controller.h
View File

@@ -258,6 +258,7 @@ class SensorItem {
class ServerRegister {
using ReadLambda = std::function<int64_t()>;
using WriteLambda = std::function<bool(int64_t value)>;
public:
ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count) {
@@ -277,6 +278,17 @@ class ServerRegister {
};
}
template<typename T>
void set_write_lambda(const std::function<bool(uint16_t address, const T v)> &&user_write_lambda) {
this->write_lambda = [this, user_write_lambda](int64_t number) {
if constexpr (std::is_same_v<T, float>) {
float float_value = bit_cast<float>(static_cast<uint32_t>(number));
return user_write_lambda(this->address, float_value);
}
return user_write_lambda(this->address, static_cast<T>(number));
};
}
// Formats a raw value into a string representation based on the value type for debugging
std::string format_value(int64_t value) const {
switch (this->value_type) {
@@ -304,6 +316,7 @@ class ServerRegister {
SensorValueType value_type{SensorValueType::RAW};
uint8_t register_count{0};
ReadLambda read_lambda;
WriteLambda write_lambda;
};
// ModbusController::create_register_ranges_ tries to optimize register range
@@ -485,6 +498,8 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
void on_modbus_error(uint8_t function_code, uint8_t exception_code) override;
/// called when a modbus request (function code 0x03 or 0x04) was parsed without errors
void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers) final;
/// called when a modbus request (function code 0x06 or 0x10) was parsed without errors
void on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data) final;
/// default delegate called by process_modbus_data when a response has retrieved from the incoming queue
void on_register_data(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data);
/// default delegate called by process_modbus_data when a response for a write response has retrieved from the

13
tests/components/modbus_controller/common.yaml
View File

@@ -33,7 +33,18 @@ modbus_controller:
read_lambda: |-
return 42.3;
max_cmd_retries: 0
- id: modbus_controller3
address: 0x3
modbus_id: mod_bus2
server_registers:
- address: 0x0009
value_type: S_DWORD
read_lambda: |-
return 31;
write_lambda: |-
printf("address=%d, value=%d", x);
return true;
max_cmd_retries: 0
binary_sensor:
- platform: modbus_controller
modbus_controller_id: modbus_controller1