From 8f701ce81ac22a3596bec9b8a20877f8202d06a2 Mon Sep 17 00:00:00 2001 From: Emanuele Trabattoni Date: Thu, 10 Jul 2025 16:01:10 +0200 Subject: [PATCH] Modbus Driver fixing multiRequest --- lib/GPIO/TCA9554PWR_Driver.cpp | 5 ++ lib/GPIO/TCA9554PWR_Driver.h | 1 + lib/RS485/RS485_Driver.cpp | 136 +++++++++++++---------------- lib/RS485/RS485_Driver.h | 6 +- lib/utils/utils.cpp | 37 ++++++++ lib/utils/utils.h | 13 +++ src/main.cpp | 151 +++++++++------------------------ 7 files changed, 158 insertions(+), 191 deletions(-) create mode 100644 lib/utils/utils.cpp create mode 100644 lib/utils/utils.h diff --git a/lib/GPIO/TCA9554PWR_Driver.cpp b/lib/GPIO/TCA9554PWR_Driver.cpp index 67511fe..0bd354e 100644 --- a/lib/GPIO/TCA9554PWR_Driver.cpp +++ b/lib/GPIO/TCA9554PWR_Driver.cpp @@ -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})) diff --git a/lib/GPIO/TCA9554PWR_Driver.h b/lib/GPIO/TCA9554PWR_Driver.h index 356cc60..61961cf 100644 --- a/lib/GPIO/TCA9554PWR_Driver.h +++ b/lib/GPIO/TCA9554PWR_Driver.h @@ -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); diff --git a/lib/RS485/RS485_Driver.cpp b/lib/RS485/RS485_Driver.cpp index 0bc0588..2155c41 100644 --- a/lib/RS485/RS485_Driver.cpp +++ b/lib/RS485/RS485_Driver.cpp @@ -3,44 +3,9 @@ #include #include -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 &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 &data) { - data.resize(nBytes); - if (m_serial.readBytes(data.data(), nBytes) == nBytes) + std::vector 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 &in) + const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector &in) { - std::vector bitsOut; - if (bits <= 1) // if single coil value must be 0x00FF[00] for on[off] + const uint16_t bits(in.size()); + std::vector 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 &in) + const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector &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 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 MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector &data) + const std::vector MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector &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 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 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; } diff --git a/lib/RS485/RS485_Driver.h b/lib/RS485/RS485_Driver.h index bbbd5fa..d68ea39 100644 --- a/lib/RS485/RS485_Driver.h +++ b/lib/RS485/RS485_Driver.h @@ -91,11 +91,11 @@ namespace drivers private: CRC16 m_crc; const std::vector singleRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t data); - const std::vector multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector &data); + const std::vector multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector &data); const bool readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector &out); const bool readInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, std::vector &out); - const bool writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, const std::vector &in); - const bool writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, const std::vector &in); + const bool writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector &in); + const bool writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector &in); const bool verifyCrc(const std::vector &data); }; } diff --git a/lib/utils/utils.cpp b/lib/utils/utils.cpp new file mode 100644 index 0000000..fe50deb --- /dev/null +++ b/lib/utils/utils.cpp @@ -0,0 +1,37 @@ +#include "utils.h" + +void printBytes(const char title[], const std::vector &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 &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 &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(); +} diff --git a/lib/utils/utils.h b/lib/utils/utils.h new file mode 100644 index 0000000..3be02d2 --- /dev/null +++ b/lib/utils/utils.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include +#include + +///////////// UTIL Functions ///////////////// + +void printBytes(const char title[], const std::vector &b); + +void printBytes(const char title[], const std::vector &b); + +void printBool(const char title[], const std::vector &vals); diff --git a/src/main.cpp b/src/main.cpp index d0c3991..3497847 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,99 +6,16 @@ #include #include -#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 results; - LOG_INFO("Set Slave Address"); - /* - while (false & !bus.writeRegister(devAddress, 0x00FE, 0x00AA)){ - std::vector 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 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 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 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 results; std::vector 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); } \ No newline at end of file