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

5
lib/GPIO/TCA9554PWR_Driver.cpp
View File

@@ -9,6 +9,11 @@ namespace drivers
writeRegister(TCA9554_CONFIG_REG, TCA9554_OUT_MODE); // set all pins as output (relay mode for this board)
}
TCA9554PWR::~TCA9554PWR() {
writeRegister(TCA9554_OUTPUT_REG, Low); // set all pins to Low state
writeRegister(TCA9554_CONFIG_REG, TCA9554_OUT_MODE); // set all pins as output (relay mode for this board)
}
const bool TCA9554PWR::writeRegister(const uint8_t reg, const uint8_t val)
{
if (m_i2c.write(m_address, reg, {val}))

1
lib/GPIO/TCA9554PWR_Driver.h
View File

@@ -35,6 +35,7 @@ namespace drivers
};
TCA9554PWR(I2C &i2c, const uint8_t address);
~TCA9554PWR();
const bool setOut(const uint8_t channel, const bool state);
const bool setPort(const uint8_t state);

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;
}

6
lib/RS485/RS485_Driver.h
View File

@@ -91,11 +91,11 @@ namespace drivers
private:
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 std::vector<uint8_t> multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint8_t> &data);
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 writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<bool> &in);
const bool writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<uint16_t> &in);
const bool verifyCrc(const std::vector<uint8_t> &data);
};
}

37
lib/utils/utils.cpp Normal file
View File

@@ -0,0 +1,37 @@
#include "utils.h"
void printBytes(const char title[], const std::vector<uint8_t> &b)
{
Serial0.flush();
printf("%s: ", title);
for (auto v : b)
{
printf("0x%02x ", v);
}
printf("\n");
Serial0.flush();
}
void printBytes(const char title[], const std::vector<uint16_t> &b)
{
Serial0.flush();
printf("%s: ", title);
for (auto v : b)
{
printf("0x%04x ", v);
}
printf("\n");
Serial0.flush();
}
void printBool(const char title[], const std::vector<bool> &vals)
{
Serial0.flush();
printf("%s: ", title);
for (auto j(0); j < vals.size(); j++)
{
printf("%s ", vals.at(j) ? "True" : "False");
}
printf("\n");
Serial0.flush();
}

13
lib/utils/utils.h Normal file
View File

@@ -0,0 +1,13 @@
#pragma once
#include <Arduino.h>
#include <DebugLog.h>
#include <vector>
///////////// UTIL Functions /////////////////
void printBytes(const char title[], const std::vector<uint8_t> &b);
void printBytes(const char title[], const std::vector<uint16_t> &b);
void printBool(const char title[], const std::vector<bool> &vals);

151
src/main.cpp
View File

@@ -6,99 +6,16 @@
#include <RS485_Driver.h>
#include <TCA9554PWR_Driver.h>
#ifdef ESP32
auto bus = drivers::MODBUS(9600, SERIAL_8N1);
#else
auto bus = drivers::MODBUS(9600);
#endif
#include "utils.h"
/////////////// GLOBALS ///////////////
uint16_t k(0);
bool v(true);
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);
success &= relays.setOut(i, true);
delay(1000);
LOG_INFO("Toggle relay [%d]=OFF", i);
success &= relays.setOut(i, false);
delay(1000);
}
LOG_INFO("Toggle port [0x55]");
success &= relays.setPort(0x55);
delay(2000);
LOG_INFO("Toggle port [0xAA]");
success &= relays.setPort(0xAA);
delay(2000);
*/
LOG_INFO("Create modbus driver");
const uint8_t devAddress(0x01);
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);
success &= bus.writeCoil(devAddress, baseRegister, false);
LOG_INFO("Write multiple coils");
const uint16_t coilsNum(32);
std::vector<bool> coilsValues(coilsNum, false);
bool v = true;
for (auto i(0); i < coilsNum; i++)
{
coilsValues[i] = v;
v = ~v;
}
success &= bus.writeCoils(devAddress, baseRegister, coilsValues);
LOG_INFO("Write single register");
success &= bus.writeRegister(devAddress, baseRegister, 0xAA);
LOG_INFO("Write multiple registers");
const uint16_t regNum(16);
std::vector<uint16_t> regValues(regNum, 0);
for (uint16_t i(0); i < regNum; i++)
{
regValues[i] = i * 2;
}
*/
}
void loop()
@@ -106,30 +23,44 @@ void loop()
const uint8_t tempBoardAddr(0xAA);
const uint8_t relayBoardAddr(0x01);
const uint8_t baseRegister(0x00);
LOG_INFO("Looop");
std::vector<uint16_t> results;
bool success = bus.readHoldingRegisters(tempBoardAddr, baseRegister, 8, results);
if (success)
//////////////// DEVICES ////////////////
auto i2c = drivers::I2C();
auto relays = drivers::TCA9554PWR(i2c, TCA9554_ADDRESS);
auto bus = drivers::MODBUS(9600, SERIAL_8N1);
while (true)
{
for (auto i(0); i < results.size(); i++)
{
LOG_INFO("[", i, "]Temperature: ", results.at(i) / 10.0f);
}
results.clear();
}
delay(100);
for (auto i(0); i < 8; i++)
{
LOG_DEBUG("\n\nCHANNEL ", i);
LOG_INFO("\n\n\n\n[", k++, "] Loop");
std::vector<uint16_t> results;
std::vector<bool> values;
bus.writeCoil(relayBoardAddr, (uint8_t)i, true);
bus.readCoils(relayBoardAddr,0x00,8, values);
for (auto j(0); j < values.size(); j++) {
LOG_DEBUG("Coil", j, values.at(j) ? "True" : "False");
if (bus.readHoldingRegisters(tempBoardAddr, baseRegister, 8, results))
{
for (auto i(0); i < results.size(); i++)
{
LOG_INFO("[", i, "]Temperature: ", results.at(i) / 10.0f);
}
results.clear();
}
delay(1000);
bus.writeCoil(relayBoardAddr, (uint8_t)i, false);
delay(1000);
delay(100);
LOG_INFO("\n\n====>Write Status 1");
bus.writeCoils(relayBoardAddr, 0x00, {true, false, true, false, true, false, true, false});
bus.readCoils(relayBoardAddr, 0x00, 8, values);
printBool("====>Status 1", values);
delay(5000);
LOG_INFO("\n\n====>Write Status 2");
bus.writeCoils(relayBoardAddr, 0x00, {false, true, false, true, false, true, false, true});
bus.readCoils(relayBoardAddr, 0x00, 8, values);
printBool("====>Status 2", values);
delay(5000);
LOG_INFO("\n\n====>Read Inputs");
bus.readInputs(relayBoardAddr, 0x00, 8, values);
printBool("====>Inputs", values);
delay(5000);
}
delay(5000);
}