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

Modbus Driver fixing multiRequest

Browse Source
This commit is contained in:
Emanuele Trabattoni
2025-07-10 16:01:10 +02:00
parent 4b97e6535d
commit 8f701ce81a
7 changed files with 158 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
lib/RS485/RS485_Driver.cpp
View File

@@ -3,44 +3,9 @@
#include <cstring>
#include <endian.h>
uint8_t data[][8] = {
// ESP32-S3-POE-ETH-8DI-8RO Control Command (RS485 receiving data)
{0x06, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA2, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH1 Toggle
{0x06, 0x05, 0x00, 0x02, 0x55, 0x00, 0x52, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH2 Toggle
{0x06, 0x05, 0x00, 0x03, 0x55, 0x00, 0x03, 0x2D}, // ESP32-S3-POE-ETH-8DI-8RO CH3 Toggle
{0x06, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB2, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH4 Toggle
{0x06, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE3, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH5 Toggle
{0x06, 0x05, 0x00, 0x06, 0x55, 0x00, 0x13, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH6 Toggle
{0x06, 0x05, 0x00, 0x07, 0x55, 0x00, 0x42, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH7 Toggle
{0x06, 0x05, 0x00, 0x08, 0x55, 0x00, 0x72, 0xEF}, // ESP32-S3-POE-ETH-8DI-8RO CH8 Toggle
{0x06, 0x05, 0x00, 0xFF, 0xFF, 0x00, 0xBD, 0xBD}, // ESP32-S3-POE-ETH-8DI-8RO ALL ON
{0x06, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFC, 0x4D}, // ESP32-S3-POE-ETH-8DI-8RO ALL OFF
};
uint8_t Send_Data[][8] = {
// Modbus RTU Relay Control Command (RS485 send data)
{0x01, 0x05, 0x00, 0x00, 0x55, 0x00, 0xF2, 0x9A}, // Modbus RTU Relay CH1 Toggle
{0x01, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA3, 0x5A}, // Modbus RTU Relay CH2 Toggle
{0x01, 0x05, 0x00, 0x02, 0x55, 0x00, 0x53, 0x5A}, // Modbus RTU Relay CH3 Toggle
{0x01, 0x05, 0x00, 0x03, 0x55, 0x00, 0x02, 0x9A}, // Modbus RTU Relay CH4 Toggle
{0x01, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB3, 0x5B}, // Modbus RTU Relay CH5 Toggle
{0x01, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE2, 0x9B}, // Modbus RTU Relay CH6 Toggle
{0x01, 0x05, 0x00, 0x06, 0x55, 0x00, 0x12, 0x9B}, // Modbus RTU Relay CH7 Toggle
{0x01, 0x05, 0x00, 0x07, 0x55, 0x00, 0x43, 0x5B}, // Modbus RTU Relay CH8 Toggle
{0x01, 0x05, 0x00, 0xFF, 0xFF, 0xFF, 0xFC, 0x4A}, // Modbus RTU Relay ALL ON
{0x01, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFD, 0xFA}, // Modbus RTU Relay ALL OFF
};
#define DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
// #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();
}
#include "utils.h"
namespace drivers
{
@@ -49,7 +14,7 @@ namespace drivers
//////////// RS485 ////////////
////////////////////////////////
RS485::RS485(const uint32_t baud, const SerialConfig conf): m_serial(Serial1)
RS485::RS485(const uint32_t baud, const SerialConfig conf) : m_serial(Serial1)
{
LOG_INFO("Init serial port 1");
// RS485 is hardwired to serial port 1
@@ -73,9 +38,13 @@ namespace drivers
const bool RS485::readN(const uint16_t nBytes, std::vector<uint8_t> &data)
{
data.resize(nBytes);
if (m_serial.readBytes(data.data(), nBytes) == nBytes)
std::vector<uint8_t> buf;
buf.resize(nBytes);
if (m_serial.readBytes(buf.data(), nBytes) == nBytes)
{
data = std::move(buf);
return true;
}
return false;
}
@@ -136,7 +105,7 @@ namespace drivers
{
constexpr uint8_t func = 0x05;
LOG_DEBUG("Write single coil: dev[", device, "], coil[", coil, "], value[", value ? "true" : "false", "]");
return writeBinary(device, func, coil, 1, {value});
return writeBinary(device, func, coil, {value});
}
// Func 0x06
@@ -144,7 +113,7 @@ namespace drivers
{
constexpr uint8_t func = 0x06;
LOG_DEBUG("Write single register: dev[", device, "], reg[", reg, "], value[", value, "]");
return writeInteger(device, func, reg, 1, {value});
return writeInteger(device, func, reg, {value});
}
// Func 0x0F
@@ -152,7 +121,7 @@ namespace drivers
{
constexpr uint8_t func = 0x0F;
LOG_DEBUG("Write multi coils: dev[", device, "], start[", coils, "], num[", values.size(), "]");
return writeBinary(device, func, coils, values.size(), values);
return writeBinary(device, func, coils, values);
}
// Func 0x10
@@ -160,7 +129,7 @@ namespace drivers
{
constexpr uint8_t func = 0x10;
LOG_DEBUG("Write multi registers: dev[", device, "], start[", reg, "], num[", values.size(), "]");
return writeInteger(device, func, reg, values.size(), values);
return writeInteger(device, func, reg, values);
}
/////////////////////////////////////////////////////////////////
@@ -188,8 +157,9 @@ namespace drivers
if (actualRespLen != nRespDataBytes)
{
LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", nRespDataBytes, "]");
LOG_DEBUG("readBinary Response");
printBytes(response);
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
printBytes("readBinary Response", response);
#endif
return false;
}
@@ -229,8 +199,9 @@ namespace drivers
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive readInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
LOG_DEBUG("readInteger Response");
printBytes(response);
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
printBytes("readInteger Response", response);
#endif
return false;
}
@@ -261,10 +232,11 @@ namespace drivers
return true;
}
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)
const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<bool> &in)
{
std::vector<uint16_t> bitsOut;
if (bits <= 1) // if single coil value must be 0x00FF[00] for on[off]
const uint16_t bits(in.size());
std::vector<uint8_t> bitsOut;
if (bits == 1) // if single coil value must be 0x00FF[00] for on[off]
{
if (!write(singleRequest(device, func, reg, in.front() ? 0xFF00 : 0x0000)))
{
@@ -274,15 +246,19 @@ namespace drivers
}
else // if multiple coils value is 0x01 shifted for the number of coil intended
{
const uint16_t numRegisters((uint16_t)(bits / 16) + 1);
bitsOut.resize(numRegisters, 0);
const uint16_t numBytes((uint16_t)ceil(bits / 8.0f));
bitsOut.resize(numBytes, 0x00);
for (uint16_t i(0); i < in.size(); i++)
{
if (!in[i]) // if value is false skip
continue;
const uint16_t curReg(i / 16);
bitsOut[curReg] |= 0x01 << i % 16;
bitsOut[i / 8] |= 0x01 << i % 8;
}
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("\nnumBytes", numBytes);
printBool("bitsOut", in);
printBytes("bitsOut", bitsOut);
#endif
if (!write(multiRequest(device, func, reg, bits, bitsOut)))
{
LOG_ERROR("Failed send writeMultiBinary command");
@@ -295,8 +271,9 @@ namespace drivers
if (!readN(expectedRespLen, response))
{
LOG_ERROR("Failed receive writeBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
LOG_DEBUG("writeBinary Response");
printBytes(response);
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
printBytes("writeBinary Response", response);
#endif
return false;
}
@@ -307,9 +284,10 @@ namespace drivers
return true;
}
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)
const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<uint16_t> &in)
{
if (in.size() == 1)
const uint16_t num(in.size());
if (num == 1)
{
if (!write(singleRequest(device, func, reg, in[0])))
{
@@ -319,7 +297,17 @@ namespace drivers
}
else
{
if (!write(multiRequest(device, func, reg, num, in)))
// build data vector for request, inverting bytes if necessary
std::vector<uint8_t> requestData;
requestData.resize(in.size() * sizeof(uint16_t));
auto it=requestData.begin();
std::for_each(in.begin(), in.end(), [requestData, &it](auto inV) {
const uint16_t beV(htobe16(inV));
*it=highByte(beV);
*(++it)=lowByte(beV);
});
if (!write(multiRequest(device, func, reg, num, requestData)))
{
LOG_ERROR("Failed send writeMultiInteger command");
return false;
@@ -332,8 +320,7 @@ namespace drivers
{
LOG_ERROR("Failed receive writeInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("writeInteger Response");
printBytes(response);
printBytes("writeInteger Response", response);
#endif
return false;
}
@@ -366,46 +353,39 @@ namespace drivers
std::memcpy(dataOut.data() + headerBytes, &crc, crcBytes);
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
LOG_DEBUG("singleRequest");
printBytes(dataOut);
printBytes("singleRequest", dataOut);
#endif
return dataOut;
}
const std::vector<uint8_t> MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint16_t> &data)
const std::vector<uint8_t> MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint8_t> &data)
{
req_multi_t header;
header.device = device;
header.func = func;
header.reg = htobe16(reg);
header.qty = htobe16(qty);
header.bytes = (uint8_t)(data.size() * sizeof(uint16_t)); // 8 bit value
header.bytes = data.size(); // 8 bit value
// convert uint16_t values from host endianness to big endian
std::vector<uint16_t> dataBe;
dataBe.reserve(data.size());
std::for_each(data.begin(), data.end(), [&dataBe](auto v)
{ dataBe.push_back(htobe16(v)); });
const uint8_t headerBytes(sizeof(req_multi_t));
const uint8_t dataBytes(sizeof(uint16_t) * dataBe.size());
//const uint8_t headerBytes(sizeof(req_multi_t)); // sizeof not working because of memory padding
const uint8_t headerBytes(7);
const uint8_t dataBytes(data.size());
const uint8_t crcBytes(sizeof(crc_t));
// compute crc for header + data
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);
m_crc.add((uint8_t *)data.data(), dataBytes);
const uint16_t crc(htole16(m_crc.calc()));
std::vector<uint8_t> dataOut;
dataOut.resize(headerBytes + dataBytes + crcBytes); // header message + data values + crc code
std::memcpy(dataOut.data(), &header, headerBytes); // copy message
std::memcpy(dataOut.data() + headerBytes, dataBe.data(), dataBytes); // copy data
std::memcpy(dataOut.data() + headerBytes, data.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);
printBytes("multiRequest", dataOut);
#endif
return dataOut;
}