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;

8
lib/RS485/RS485_Driver.h
View File

@@ -99,10 +99,10 @@ namespace drivers
CRC16 m_crc;
const std::vector<uint8_t> singleRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t data);
const std::vector<uint8_t> multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint16_t> &data);
const bool readBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, std::vector<bool> &out);
const bool readInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out);
const bool writeBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in);
const bool 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 readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector<bool> &out);
const bool readInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out);
const bool writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in);
const bool writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, const std::vector<uint16_t> &in);
const bool verifyCrc(const std::vector<uint8_t> &data);
};
}

75
src/main.cpp
View File

@@ -1,18 +1,29 @@
#define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG
#include <DebugLog.h>
#include <DebugLogEnable.h>
#include <Arduino.h>
#include <RS485_Driver.h>
#include <TCA9554PWR_Driver.h>
#ifdef ESP32
auto bus = drivers::MODBUS(9600, SERIAL_8N1);
#else
auto bus = drivers::MODBUS(9600);
#endif
void setup()
{
bool success = true;
Serial.begin(9600);
LOG_ATTACH_SERIAL(Serial);
LOG_INFO("Create i2c driver");
auto i2c = drivers::I2C();
LOG_INFO("Create relay driver");
auto relays = drivers::TCA9554PWR(i2c, TCA9554_ADDRESS);
/*
for (auto i(0); i < drivers::TCA9554PWR::OUT_PIN_MAX; i++)
{
LOG_INFO("Toggle relay [%d]=ON", i);
@@ -29,15 +40,38 @@ void setup()
LOG_INFO("Toggle port [0xAA]");
success &= relays.setPort(0xAA);
delay(2000);
*/
LOG_INFO("Create modbus driver");
#ifdef ESP32
auto bus = drivers::MODBUS(9600, SERIAL_8N1);
#else
auto bus = drivers::MODBUS(9600);
#endif
const uint8_t devAddress(0x01);
const uint8_t baseRegister(0x02);
const uint8_t baseRegister(0x00);
std::vector<uint16_t> results;
LOG_INFO("Set Slave Address");
/*
while (false & !bus.writeRegister(devAddress, 0x00FE, 0x00AA)){
std::vector<uint16_t> rv;
if(bus.readHoldingRegisters(0x00AA, 0x00FE, 1, rv)) {
LOG_INFO("New Slave address: ", rv[0]);
break;
}
delay(500);
};
*/
/*
while (true)
{
success &= bus.readHoldingRegisters(devAddress, baseRegister, 8, results);
if (success) {
for (auto i(0); i< results.size(); i++){
LOG_INFO("[",i,"]Temperature: ", results.at(i)/10.0f);
}
results.clear();
}
delay(2000);
}
LOG_INFO("Write single coil");
success &= bus.writeCoil(devAddress, baseRegister, true);
@@ -64,8 +98,37 @@ void setup()
{
regValues[i] = i * 2;
}
*/
}
void loop()
{
const uint8_t devAddress(0xAA);
const uint8_t baseRegister(0x00);
LOG_INFO("Looop");
std::vector<uint16_t> results;
bool success = bus.readHoldingRegisters(devAddress, baseRegister, 8, results);
if (success)
{
for (auto i(0); i < results.size(); i++)
{
LOG_INFO("[", i, "]Temperature: ", results.at(i) / 10.0f);
}
results.clear();
}
for (auto i(0); i < 8; i++)
{
LOG_DEBUG("\nCHANNEL ", i);
std::vector<bool> values;
bus.writeCoil(0x01, (uint8_t)i, true);
delay(500);
bus.readCoils(0x01,0x00,8, values);
for (auto v: values ) {
LOG_DEBUG("Coil", i, v ? "True" : "False");
}
bus.writeCoil(0x01, (uint8_t)i, false);
delay(500);
}
delay(5000);
}