polymech/firmware-base
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mb tcp fix

Browse Source
This commit is contained in:
lovebird 2025-05-25 10:10:01 +02:00
parent 69a8b3873e
commit 0897a34fae
2 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/App.cpp
View File

@ -135,7 +135,7 @@ short App::info()
return E_OK;
}
Component *App::byId(ushort id)
Component *App::byId(ushort id)
{
short s = components.size();
for (short i = 0; i < s; i++)

35
src/modbus/ModbusTCP.cpp
View File

@ -117,13 +117,14 @@ short ModbusTCP::setup()
// READ_HOLDING_REGISTERS worker (FC=03)
auto readHregsWorker = [this](ModbusMessage request) -> ModbusMessage
{
Log.verboseln("ModbusTCP: Read HReg worker called");
ModbusMessage response;
uint16_t address = 0;
uint16_t count = 0;
request.get(2, address);
request.get(4, count);
if (count == 0 || count > 125)
{
{
Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - Invalid count %d", count, address);
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_VALUE);
return response;
@ -133,12 +134,23 @@ short ModbusTCP::setup()
{
uint16_t currentAddr = address + i;
MB_Registers *reg = this->findMappingForAddress(currentAddr, FN_READ_HOLD_REGISTER);
uint16_t regValue = 0; // Default to 0
Component *target = owner->byId(reg->componentId);
if (target)
if (reg)
{
regValue = (uint16_t)target->mb_tcp_read(reg);
response.add(regValue);
uint16_t regValue = 0;
Component *target = owner->byId(reg->componentId);
if (target)
{
regValue = (uint16_t)target->mb_tcp_read(reg);
response.add(regValue);
}
else
{
//Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - Component ID %d not found", count, address, reg->componentId);
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_ADDRESS);
}
}else{
//Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - No mapping found", count, address);
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_ADDRESS);
}
}
return response;
@ -180,7 +192,7 @@ short ModbusTCP::setup()
short result = target->mb_tcp_write(reg, value ? 1 : 0);
if (result == E_OK)
{
response = request; // Echo request on success
response = request;
}
else
{
@ -221,11 +233,10 @@ short ModbusTCP::setup()
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Internal error
return response;
}
short result = target->mb_tcp_write(reg, (short)value);
if (result == E_OK)
{
response = request; // Echo request on success
response = request;
}
else
{
@ -268,7 +279,7 @@ short ModbusTCP::setup()
{
Log.errorln("ModbusTCP: Write Multi Coil FC=15 Addr=%d Count=%d - Failed to read data byte at index %d.", address, count, dataIndex - 1);
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Or ILLEGAL_DATA_VALUE
return response; // Abort on data read error
return response; // Abort on data read error
}
}
@ -344,7 +355,7 @@ short ModbusTCP::setup()
}
bool anyWriteFailed = false; // Track actual component write failures
uint8_t dataIndex = 7; // Start of register data in request
uint8_t dataIndex = 7; // Start of register data in request
for (uint16_t i = 0; i < count; ++i)
{
@ -355,7 +366,7 @@ short ModbusTCP::setup()
{
Log.errorln("ModbusTCP: Write Multi HReg FC=16 Addr=%d Count=%d - Failed to read data value at index %d.", address, count, dataIndex);
response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Or ILLEGAL_DATA_VALUE
return response; // Abort on data read error
return response; // Abort on data read error
}
dataIndex += 2;