From f274970d63ba9864bc4a105d53b5ec91e9c9d6f5 Mon Sep 17 00:00:00 2001 From: Emanuele Trabattoni Date: Wed, 2 Jul 2025 18:45:57 +0200 Subject: [PATCH] Major fixes to MODBUS Driver --- lib/RS485/RS485_Driver.cpp | 168 ++++++++++++++++++++++++------------- lib/RS485/RS485_Driver.h | 8 +- src/main.cpp | 75 +++++++++++++++-- 3 files changed, 185 insertions(+), 66 deletions(-) diff --git a/lib/RS485/RS485_Driver.cpp b/lib/RS485/RS485_Driver.cpp index 96e071e..9228ed1 100644 --- a/lib/RS485/RS485_Driver.cpp +++ b/lib/RS485/RS485_Driver.cpp @@ -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 &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(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(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 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 &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 &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 &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 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 &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 &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 &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 &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 &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 &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 &out) + const bool MODBUS::readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector &out) { if (!write(singleRequest(device, func, reg, bits))) { @@ -182,14 +201,17 @@ namespace drivers std::vector 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 respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE); + const std::vector 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 &out) + const bool MODBUS::readInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, std::vector &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 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 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 &in) + const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, const std::vector &in) { std::vector 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 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 &in) + const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, const std::vector &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 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 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 &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; diff --git a/lib/RS485/RS485_Driver.h b/lib/RS485/RS485_Driver.h index 93a2c20..4f51b71 100644 --- a/lib/RS485/RS485_Driver.h +++ b/lib/RS485/RS485_Driver.h @@ -99,10 +99,10 @@ namespace drivers 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 bool readBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, std::vector &out); - const bool readInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, std::vector &out); - const bool writeBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, const std::vector &in); - const bool writeInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, const std::vector &in); + 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 verifyCrc(const std::vector &data); }; } diff --git a/src/main.cpp b/src/main.cpp index e0f35ae..2ea3650 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,18 +1,29 @@ #define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG #include +#include #include #include #include +#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 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); @@ -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 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 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); } \ No newline at end of file