Obbart/ETcontroller_PRO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Major fixes to MODBUS Driver

Browse Source
This commit is contained in:
Emanuele Trabattoni
2025-07-02 18:45:57 +02:00
parent 3d2d44c0bb
commit f274970d63
3 changed files with 185 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
lib/RS485/RS485_Driver.cpp
View File

@@ -37,6 +37,18 @@ uint8_t Send_Data[][8] = {
{0x01, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFD, 0xFA}, // Modbus RTU Relay ALL OFF
};
#define DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
void printBytes(const std::vector<uint8_t> &b)
{
Serial0.flush();
for (auto v : b)
{
printf("0x%02x ", v);
}
printf("\n");
Serial0.flush();
}
namespace drivers
{
@@ -48,9 +60,9 @@ namespace drivers
RS485::RS485(const uint32_t baud, const SerialConfig conf)
{
LOG_INFO("Init serial port 1");
m_serial = std::make_unique<HardwareSerial>(PORT); // RS485 is hardwired to serial port 1
m_serial->begin(baud, conf);
m_serial->setMode(UART_MODE_RS485_HALF_DUPLEX);
// m_serial = std::make_unique<HardwareSerial>(1); // RS485 is hardwired to serial port 1
Serial1.begin(baud, conf, 18, 17);
Serial1.flush();
}
#else
@@ -63,12 +75,14 @@ namespace drivers
#endif
const bool RS485::write(const std::vector<uint8_t> data)
{
return data.size() == m_serial->write(data.data(), data.size());
return data.size() == Serial1.write(data.data(), data.size());
}
const bool RS485::readAll(std::vector<uint8_t> &data)
{
const uint32_t avail(m_serial->available());
const uint32_t avail(Serial1.available());
if (avail == 0)
return true;
data.resize(avail);
return data.size() == m_serial->readBytes(data.data(), avail);
}
@@ -76,14 +90,16 @@ namespace drivers
const bool RS485::readN(const uint16_t nBytes, std::vector<uint8_t> &data)
{
data.resize(nBytes);
return data.size() == m_serial->readBytes(data.data(), nBytes);
if (Serial1.readBytes(data.data(), nBytes) == nBytes)
return true;
return false;
}
const bool RS485::readUntil(const uint8_t ch, std::vector<uint8_t> &data)
{
const uint32_t avail(m_serial->available());
const uint32_t avail(Serial1.available());
data.resize(avail);
m_serial->readBytesUntil(ch, data.data(), avail);
Serial1.readBytesUntil(ch, data.data(), avail);
data.shrink_to_fit();
return true;
}
@@ -94,7 +110,10 @@ namespace drivers
#ifdef ESP32
MODBUS::MODBUS(const uint32_t baud, const SerialConfig conf) : RS485::RS485(baud, conf)
{
std::vector<uint8_t> garbage;
readAll(garbage);
LOG_INFO("Init MODBUS Master Mode");
m_crc.reset(CRC16_MODBUS_POLYNOME, CRC16_MODBUS_INITIAL, CRC16_MODBUS_XOR_OUT, CRC16_MODBUS_REV_IN, CRC16_MAXIM_REV_OUT);
}
#else
MODBUS::MODBUS(const uint32_t baud) : RS485::RS485(baud)
@@ -106,39 +125,39 @@ namespace drivers
const bool MODBUS::readCoils(const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<bool> &coils)
{
constexpr uint8_t func = 0x01;
LOG_DEBUG("Read coils: dev[%02x], reg[%04x], num[%d]", device, reg, num);
return readBinary(func, device, reg, num, coils);
LOG_DEBUG("Read coils: dev[", device, "], reg[", reg, "], num[", num, "]");
return readBinary(device, func, reg, num, coils);
}
// Func 0x02
const bool MODBUS::readInputs(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<bool> &inputs)
{
constexpr uint8_t func = 0x01;
LOG_DEBUG("Read multi inputs: dev[%02x], reg[%04x], num[%d]", device, reg, num);
return readBinary(func, device, reg, num, inputs);
constexpr uint8_t func = 0x02;
LOG_DEBUG("Read multi inputs: dev[", device, "], reg[", reg, "], num[", num, "]");
return readBinary(device, func, reg, num, inputs);
}
// Func 0x03
const bool MODBUS::readHoldingRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
{
constexpr uint8_t func = 0x03;
LOG_DEBUG("Read multi holding registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
return readInteger(func, device, reg, num, values);
LOG_DEBUG("Read multi holding registers: dev[", device, "], reg[", reg, "], num[", num, "]");
return readInteger(device, func, reg, num, values);
}
// Func 0x04
const bool MODBUS::readInputRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
{
constexpr uint8_t func = 0x04;
LOG_DEBUG("Read multi input registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
return readInteger(func, device, reg, num, values);
LOG_DEBUG("Read multi input registers: dev[", device, "], reg[", reg, "], num[", num, "]");
return readInteger(device, func, reg, num, values);
}
// Func 0x05
const bool MODBUS::writeCoil(const uint8_t device, const uint16_t coil, const bool value)
{
constexpr uint8_t func = 0x05;
LOG_DEBUG("Write single coil: dev[%02x], reg[%04x], val[...]", device, coil);
LOG_DEBUG("Write single coils: dev[", device, "], coil[", coil, "], value[", value ? "true" : "false", "]");
return writeBinary(device, func, coil, 1, {value});
}
@@ -146,7 +165,7 @@ namespace drivers
const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
{
constexpr uint8_t func = 0x06;
LOG_DEBUG("Write single register: dev[%02x], reg[%04x], val[%04x]", device, reg, value);
LOG_DEBUG("Write single register: dev[", device, "], reg[", reg, "], value[", value, "]");
return writeInteger(device, func, reg, 1, {value});
}
@@ -154,7 +173,7 @@ namespace drivers
const bool MODBUS::writeCoils(const uint8_t device, const uint16_t coils, const std::vector<bool> &values)
{
constexpr uint8_t func = 0x0F;
LOG_DEBUG("Write multi coils: dev[%02x], reg[%04x], val[...]", device, coils);
LOG_DEBUG("Write multi coils: dev[", device, "], start[", coils, "], num[", values.size(), "]");
return writeBinary(device, func, coils, values.size(), values);
}
@@ -162,7 +181,7 @@ namespace drivers
const bool MODBUS::writeRegisters(const uint8_t device, const uint16_t reg, const std::vector<uint16_t> &values)
{
constexpr uint8_t func = 0x10;
LOG_DEBUG("Write multi registers: dev[%02x], reg[%04x], val[...]", device, reg);
LOG_DEBUG("Write multi coils: dev[", device, "], start[", reg, "], num[", values.size(), "]");
return writeInteger(device, func, reg, values.size(), values);
}
@@ -170,7 +189,7 @@ namespace drivers
/////////////////////// Utility Functions ///////////////////////
/////////////////////////////////////////////////////////////////
const bool MODBUS::readBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, std::vector<bool> &out)
const bool MODBUS::readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector<bool> &out)
{
if (!write(singleRequest(device, func, reg, bits)))
{
@@ -182,14 +201,17 @@ namespace drivers
std::vector<uint8_t> response;
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive readBinary response");
LOG_ERROR("Failed receive readBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
return false;
}
// element 2 of response has the response data bytes expected
if (response.at(2) != nRespDataBytes)
const uint8_t actualRespLen(response.at(2));
if (actualRespLen != nRespDataBytes)
{
LOG_ERROR("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", nRespDataBytes, "]");
LOG_DEBUG("readBinary Response");
printBytes(response);
return false;
}
@@ -203,7 +225,7 @@ namespace drivers
uint16_t bitNum(0);
// get response data bytes excluding header and crc
const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - sizeof(crc_t));
for (auto it = respData.begin(); it < respData.end(); it++)
{
const auto v = *it;
@@ -217,7 +239,7 @@ namespace drivers
return true;
}
const bool MODBUS::readInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out)
const bool MODBUS::readInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out)
{
if (!write(singleRequest(device, func, reg, num)))
{
@@ -225,18 +247,21 @@ namespace drivers
return false;
}
const uint16_t nRespDataBytes = num * sizeof(uint16_t);
const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
const uint16_t expectedRespLen = (RESP_HEADER_SIZE + sizeof(crc_t)) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
std::vector<uint8_t> response;
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive readInteger response");
LOG_ERROR("Failed receive readInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
LOG_DEBUG("readInteger Response");
printBytes(response);
return false;
}
// element 2 of response has the response data bytes expected
if (response.at(2) != nRespDataBytes)
const uint8_t actualRespLen(response.at(2));
if (actualRespLen != nRespDataBytes)
{
LOG_ERROR("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", nRespDataBytes, "]");
return false;
}
@@ -246,25 +271,29 @@ namespace drivers
// extract coils data from data portion of response
out.clear();
out.reserve(nRespDataBytes);
out.reserve(nRespDataBytes / sizeof(uint16_t));
// get response data bytes excluding header and crc
const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
for (auto i(0); i < nRespDataBytes; i++)
for (auto it = respData.begin(); it < respData.end(); it++)
{
const uint8_t hi(respData.at(i * sizeof(uint16_t)));
const uint8_t lo(respData.at(1 + i * sizeof(uint16_t)));
const uint8_t lo(*it++);
const uint8_t hi(*it);
const uint16_t val(0xFFFF & ((hi << 8) | lo));
out.push_back(be16toh(val));
}
return true;
}
const bool MODBUS::writeBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in)
const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in)
{
std::vector<uint16_t> bitsOut;
if (bits <= 1) // if single coil value must be 0x00FF[00] for on[off]
{
bitsOut.push_back(htobe16(in.front() ? 0x00FF : 0x0000));
if (!write(singleRequest(device, func, reg, in.front() ? 0xFF00 : 0x0000)))
{
LOG_ERROR("Failed send writeSingleBinary command");
return false;
}
}
else // if multiple coils value is 0x01 shifted for the number of coil intended
{
@@ -277,19 +306,20 @@ namespace drivers
const uint16_t curReg(i / 16);
bitsOut[curReg] |= 0x01 << i % 16;
}
if (!write(multiRequest(device, func, reg, bits, bitsOut)))
{
LOG_ERROR("Failed send writeMultiBinary command");
return false;
}
}
if (!write(multiRequest(device, func, reg, bits, bitsOut)))
{
LOG_ERROR("Failed send writeBinary command");
return false;
}
const uint16_t expectedRespLen(sizeof(resp_t));
const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
std::vector<uint8_t> response;
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive writeBinary response");
LOG_ERROR("Failed receive writeBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
LOG_DEBUG("writeBinary Response");
printBytes(response);
return false;
}
@@ -300,19 +330,34 @@ namespace drivers
return true;
}
const bool MODBUS::writeInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, const std::vector<uint16_t> &in)
const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, const std::vector<uint16_t> &in)
{
if (!write(multiRequest(device, func, reg, num, in)))
if (in.size() == 1)
{
LOG_ERROR("Failed send writeInteger command");
return false;
if (!write(singleRequest(device, func, reg, in[0])))
{
LOG_ERROR("Failed send writeSingleInteger command");
return false;
}
}
else
{
if (!write(multiRequest(device, func, reg, num, in)))
{
LOG_ERROR("Failed send writeMultiInteger command");
return false;
}
}
const uint16_t expectedRespLen(sizeof(resp_t));
const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
std::vector<uint8_t> response;
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive writeInteger response");
LOG_ERROR("Failed receive writeInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("writeInteger Response");
printBytes(response);
#endif
return false;
}
@@ -335,13 +380,18 @@ namespace drivers
const uint8_t crcBytes(sizeof(crc_t));
// compute crc for header + data
m_crc.reset();
m_crc.restart();
m_crc.add((uint8_t *)&header, headerBytes); // exclude last two bytes of crc
const uint16_t crc(htole16(m_crc.calc()));
std::vector<uint8_t> dataOut(headerBytes + crcBytes, 0);
std::memcpy(dataOut.data(), &header, headerBytes);
std::memcpy(dataOut.data() + headerBytes, &crc, crcBytes);
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("singleRequest");
printBytes(dataOut);
#endif
return dataOut;
}
@@ -365,7 +415,7 @@ namespace drivers
const uint8_t crcBytes(sizeof(crc_t));
// compute crc for header + data
m_crc.reset();
m_crc.restart();
m_crc.add((uint8_t *)&header, headerBytes); // add the request excluding the CRC code
m_crc.add((uint8_t *)dataBe.data(), dataBytes);
const uint16_t crc(htole16(m_crc.calc()));
@@ -375,24 +425,30 @@ namespace drivers
std::memcpy(dataOut.data(), &header, headerBytes); // copy message
std::memcpy(dataOut.data() + headerBytes, dataBe.data(), dataBytes); // copy data
std::memcpy(dataOut.data() + headerBytes + dataBytes, &crc, crcBytes); // copy crc
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("multiRequest");
printBytes(dataOut);
#endif
return dataOut;
}
const bool MODBUS::verifyCrc(const std::vector<uint8_t> &data)
{
// compute crc of current message
m_crc.reset();
m_crc.add(data.data(), data.size());
m_crc.restart();
m_crc.add(data.data(), data.size() - sizeof(crc_t));
const uint16_t computedCrc(m_crc.calc());
// extract crc from response
const uint16_t size(data.size());
const uint8_t crcLo(data.at(size - 2));
const uint8_t crcHi(data.at(size - 1));
const uint16_t receivedCrc(0xFFFF & ((crcHi << 8) | crcLo));
// verify crc code
if (highByte(computedCrc) != crcHi || lowByte(computedCrc) != crcLo)
{
LOG_ERROR("Failed verify CRC code: comp[%04x], rec[%04x]", computedCrc, 0xFFFF & ((crcHi << 8) | crcLo));
LOG_ERROR("Failed verify CRC code: comp[", computedCrc, "], rec[", receivedCrc, "]");
return false;
}
return true;