From 64eea07d7805b2c8c9b0dcbcd77c899f97d618d7 Mon Sep 17 00:00:00 2001 From: Andrey Klimov Date: Tue, 10 May 2022 13:23:17 +0300 Subject: [PATCH] ModbusTCP&UDP option (-D IPMODBUS) --- .gitignore | 2 + lighthub/ipmodbus.cpp | 561 ++++++++++++++++++++++++++++++++++++++++ lighthub/ipmodbus.h | 91 +++++++ lighthub/main.cpp | 15 +- lighthub/main.h | 4 + lighthub/microtimer.cpp | 25 ++ lighthub/microtimer.h | 20 ++ platformio.ini | 1 + 8 files changed, 717 insertions(+), 2 deletions(-) create mode 100644 lighthub/ipmodbus.cpp create mode 100644 lighthub/ipmodbus.h create mode 100644 lighthub/microtimer.cpp create mode 100644 lighthub/microtimer.h diff --git a/.gitignore b/.gitignore index 698482d..270361b 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,8 @@ custom-build-flags/build_flags_nrf52840 .vscode/launch.json lighthub/modules/out_elevator.cpp lighthub/modules/out_elevator.h +lighthub/modules/out_humidifier.cpp +lighthub/modules/out_humidifier.h spare_files/* spare_files/ArduinoOTA/* lib/* \ No newline at end of file diff --git a/lighthub/ipmodbus.cpp b/lighthub/ipmodbus.cpp new file mode 100644 index 0000000..322b611 --- /dev/null +++ b/lighthub/ipmodbus.cpp @@ -0,0 +1,561 @@ +/* ******************************************************************* + Modbus TCP/UDP functions + Based on https://github.com/budulinek/arduino-modbus-rtu-tcp-gateway + + recvUdp + - receives Modbus UDP (or Modbus RTU over UDP) messages + - calls checkRequest + - stores requests in queue or replies with error + + recvTcp + - receives Modbus TCP (or Modbus RTU over TCP) messages + - calls checkRequest + - stores requests in queue or replies with error + + processRequests + - inserts scan request into queue + - optimizes queue + + checkRequest + - checks Modbus TCP/UDP requests (correct MBAP header, CRC in case of Modbus RTU over TCP/UDP) + - checks availability of queue + + deleteRequest + - deletes requests from queue + + getSlaveResponding, setSlaveResponding + - read from and write to bool array + + ***************************************************************** */ + +#ifdef IPMODBUS +#include "ipmodbus.h" +#include +#include +#include +#include + + +byte maxSockNum = MAX_SOCK_NUM; +EthernetUDP Udp; +EthernetServer modbusServer(502); + +// each request is stored in 3 queues (all queues are written to, read and deleted in sync) +CircularBuffer queueHeaders; // queue of requests' headers and metadata (MBAP transaction ID, MBAP unit ID, PDU length, remIP, remPort, TCP client) +CircularBuffer queuePDUs; // queue of PDU data (function code, data) +CircularBuffer queueRetries; // queue of retry counters + + +enum state serialState; +unsigned int charTimeout; +unsigned int frameDelay; + +// bool array for storing Modbus RTU status (responging or not responding). Array index corresponds to slave address. +uint8_t slavesResponding[(maxSlaves + 1 + 7) / 8]; +uint8_t masks[8] = {1, 2, 4, 8, 16, 32, 64, 128}; + +const byte scanCommand[] = {0x03, 0x00, 0x00, 0x00, 0x01}; // Command sent during Modbus RTU Scan. Slave is detected if any response (even error) is received. + + +MicroTimer rxDelay; +MicroTimer rxTimeout; +MicroTimer txDelay; + + +Timer requestTimeout; +uint16_t crc; +byte scanCounter = 0; + +/****** RUN TIME AND DATA COUNTERS ******/ + +// store uptime seconds (includes seconds counted before millis() overflow) +unsigned long seconds; +// store last millis() so that we can detect millis() overflow +unsigned long last_milliseconds = 0; +// store seconds passed until the moment of the overflow so that we can add them to "seconds" on the next call +unsigned long remaining_seconds = 0; +// Data counters (we only use unsigned long in ENABLE_EXTRA_DIAG, to save flash memory) +#ifdef ENABLE_EXTRA_DIAG +unsigned long serialTxCount = 0; +unsigned long serialRxCount = 0; +unsigned long ethTxCount = 0; +unsigned long ethRxCount = 0; +#else +unsigned int serialTxCount = 0; +unsigned int serialRxCount = 0; +unsigned int ethTxCount = 0; +unsigned int ethRxCount = 0; +#endif /* ENABLE_EXTRA_DIAG */ + +int rxNdx = 0; +int txNdx = 0; +bool rxErr = false; + + +void recvUdp() +{ + unsigned int packetSize = Udp.parsePacket(); + if (packetSize) + { + ethRxCount += packetSize; + byte udpInBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame + // Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address)..... + // Modbus RTU frame: [0] address..... + Udp.read(udpInBuffer, sizeof(udpInBuffer)); + Udp.flush(); + + byte errorCode = checkRequest(udpInBuffer, packetSize); + byte pduStart; // first byte of Protocol Data Unit (i.e. Function code) + if (enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address) + else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address) + if (errorCode == 0) { + // Store in request queue: 2 bytes MBAP Transaction ID (ignored in Modbus RTU over TCP); MBAP Unit ID (address); PDUlen (func + data);remote IP; remote port; TCP client Number (socket) - 0xFF for UDP + queueHeaders.push(header {{udpInBuffer[0], udpInBuffer[1]}, udpInBuffer[pduStart - 1], (byte)(packetSize - pduStart), Udp.remoteIP(), Udp.remotePort(), UDP_REQUEST}); + queueRetries.push(0); + for (byte i = 0; i < (byte)(packetSize - pduStart); i++) { + queuePDUs.push(udpInBuffer[i + pduStart]); + } + } else if (errorCode != 0xFF) { + // send back message with error code + Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); + if (!enableRtuOverTcp) { + Udp.write(udpInBuffer, 5); + Udp.write(0x03); + } + Udp.write(udpInBuffer[pduStart - 1]); // address + Udp.write(udpInBuffer[pduStart] + 0x80); // function + 0x80 + Udp.write(errorCode); + if (enableRtuOverTcp) { + crc = 0xFFFF; + calculateCRC(udpInBuffer[pduStart - 1]); + calculateCRC(udpInBuffer[pduStart] + 0x80); + calculateCRC(errorCode); + Udp.write(lowByte(crc)); // send CRC, low byte first + Udp.write(highByte(crc)); + } + Udp.endPacket(); + ethTxCount += 5; + if (!enableRtuOverTcp) ethTxCount += 4; + } + } +} + + +void recvTcp() +{ + EthernetClient client = modbusServer.available(); + if (client) { + unsigned int packetSize = client.available(); + ethRxCount += packetSize; + byte tcpInBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame + // Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address)..... + // Modbus RTU frame: [0] address..... + client.read(tcpInBuffer, sizeof(tcpInBuffer)); + client.flush(); + byte errorCode = checkRequest(tcpInBuffer, packetSize); + byte pduStart; // first byte of Protocol Data Unit (i.e. Function code) + if (enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address) + else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address) + //debugSerial<<"TCP modbus received packet. Code="< 0 ; j--) { // start searching from tail because requests to non-responsive slaves are usually towards the tail of the queue + if (queueHeaders[j - 1].uid == address) { + return 0x0B; // return modbus error 11 (Gateway Target Device Failed to Respond) - usually means that target device (address) is not present + } + } + } + // check if we have space in request queue + if (queueHeaders.available() < 1 || (enableRtuOverTcp && queuePDUs.available() < bufferSize - 1) || (!enableRtuOverTcp && queuePDUs.available() < bufferSize - 7)) { + return 0x06; // return modbus error 6 (Slave Device Busy) - try again later + } + // al checkes passed OK, we can store the incoming data in request queue + return 0; +} + +void deleteRequest() // delete request from queue +{ + for (byte i = 0; i < queueHeaders.first().PDUlen; i++) { + queuePDUs.shift(); + } + queueHeaders.shift(); + queueRetries.shift(); +} + + +bool getSlaveResponding(const uint8_t index) +{ + if (index >= maxSlaves) return false; // error + return (slavesResponding[index / 8] & masks[index & 7]) > 0; +} + + +void setSlaveResponding(const uint8_t index, const bool value) +{ + if (index >= maxSlaves) return; // error + if (value == 0) slavesResponding[index / 8] &= ~masks[index & 7]; + else slavesResponding[index / 8] |= masks[index & 7]; +} + +/* ******************************************************************* + Modbus RTU functions + + sendSerial + - sends Modbus RTU requests to HW serial port (RS485 interface) + + recvSerial + - receives Modbus RTU replies + - adjusts headers and forward messages as Modbus TCP/UDP or Modbus RTU over TCP/UDP + - sends Modbus TCP/UDP error messages in case Modbus RTU response timeouts + + checkCRC + - checks an array and returns true if CRC is OK + + calculateCRC + + ***************************************************************** */ + + + +void sendSerial() +{ + if (serialState == SENDING && rxNdx == 0) { // avoid bus collision, only send when we are not receiving data + if (mySerial.availableForWrite() > 0 && txNdx == 0) { + + preTransmission(); + + crc = 0xFFFF; + mySerial.write(queueHeaders.first().uid); // send uid (address) + //debugSerial.print(queueHeaders.first().uid,HEX);debugSerial.print(","); + + calculateCRC(queueHeaders.first().uid); + } + while (mySerial.availableForWrite() > 0 && txNdx < queueHeaders.first().PDUlen) { + mySerial.write(queuePDUs[txNdx]); // send func and data + //debugSerial.println(queuePDUs[txNdx],HEX);debugSerial.print(","); + + calculateCRC(queuePDUs[txNdx]); + txNdx++; + } + if (mySerial.availableForWrite() > 1 && txNdx == queueHeaders.first().PDUlen) { + // In Modbus TCP mode we must add CRC (in Modbus RTU over TCP, CRC is already in queuePDUs) + if (!enableRtuOverTcp || queueHeaders.first().clientNum == SCAN_REQUEST) { + mySerial.write(lowByte(crc)); // send CRC, low byte first + //debugSerial.println(lowByte(crc),HEX);debugSerial.print(","); + + mySerial.write(highByte(crc)); + //debugSerial.println(highByte(crc),HEX);debugSerial.println(""); + } + txNdx++; + } + if (mySerial.availableForWrite() == SERIAL_TX_BUFFER_SIZE - 1 && txNdx > queueHeaders.first().PDUlen) { + // wait for last byte (incl. CRC) to be sent from serial Tx buffer + // this if statement is not very reliable (too fast) + // Serial.isFlushed() method is needed....see https://github.com/arduino/Arduino/pull/3737 + #ifdef DEBUG + debugSerial<<"Wrote "< 0) { +/* + //this timeout fires before frameTO and broke good packet processing + if (rxTimeout.isOver() && rxNdx != 0) { + rxErr = true; // character timeout + #ifdef DEBUG + debugSerial<<"InterDigit timeout"<>"); + //Serial.println(serialIn[rxNdx],HEX); + + rxNdx++; + } else { + //debugSerial.write(">!>"); + //debugSerial.println(mySerial.read(),HEX); + rxErr = true; // frame longer than maximum allowed + } + rxDelay.sleep(frameDelay); + rxTimeout.sleep(charTimeout); + } + if (rxDelay.isOver() && rxNdx != 0) { + if (!serialIn[rxNdx-1]) rxNdx--; /// Raw hack - extra 0 byte received from some controllers + // Process Serial data + // Checks: 1) RTU frame is without errors; 2) CRC; 3) address of incoming packet against first request in queue; 4) only expected responses are forwarded to TCP/UDP + if (!rxErr && checkCRC(serialIn, rxNdx) == true && serialIn[0] == queueHeaders.first().uid && serialState == WAITING) { + #ifdef DEBUG + debugSerial << "Correct packet received from Serial:" << rxNdx << endl; + #endif + setSlaveResponding(serialIn[0], true); // flag slave as responding + byte MBAP[] = {queueHeaders.first().tid[0], queueHeaders.first().tid[1], 0x00, 0x00, highByte(rxNdx - 2), lowByte(rxNdx - 2)}; + if (queueHeaders.first().clientNum == UDP_REQUEST) { + Udp.beginPacket(queueHeaders.first().remIP, queueHeaders.first().remPort); + if (enableRtuOverTcp) Udp.write(serialIn, rxNdx); + else { + Udp.write(MBAP, 6); + Udp.write(serialIn, rxNdx - 2); //send without CRC + } + Udp.endPacket(); + ethTxCount += rxNdx; + if (!enableRtuOverTcp) ethTxCount += 4; + } else if (queueHeaders.first().clientNum != SCAN_REQUEST) { + EthernetClient client = EthernetClient(queueHeaders.first().clientNum); + // make sure that this is really our socket + if (client.localPort() == tcpPort && (client.status() == SnSR::ESTABLISHED || client.status() == SnSR::CLOSE_WAIT)) { + if (enableRtuOverTcp) client.write(serialIn, rxNdx); + else { + client.write(MBAP, 6); + client.write(serialIn, rxNdx - 2); //send without CRC + #ifdef DEBUG + debugSerial << "Packet transmitted to TCP " << rxNdx << endl; + #endif + } + ethTxCount += rxNdx; + if (!enableRtuOverTcp) ethTxCount += 4; + } + } + deleteRequest(); + serialState = MBIDLE; + modbusBusy = false; + } + #ifdef DEBUG + debugSerial << "Packet cleared. " << rxNdx << "bytes"<< endl; + debugSerial.print(">>"); + for (byte i=0;i= serialRetry) { + // send modbus error 11 (Gateway Target Device Failed to Respond) - usually means that target device (address) is not present + byte MBAP[] = {queueHeaders.first().tid[0], queueHeaders.first().tid[1], 0x00, 0x00, 0x00, 0x03}; + byte PDU[] = {queueHeaders.first().uid, (byte)(queuePDUs[0] + 0x80), 0x0B}; + crc = 0xFFFF; + for (byte i = 0; i < sizeof(PDU); i++) { + calculateCRC(PDU[i]); + } + if (queueHeaders.first().clientNum == UDP_REQUEST) { + Udp.beginPacket(queueHeaders.first().remIP, queueHeaders.first().remPort); + if (!enableRtuOverTcp) { + Udp.write(MBAP, 6); + } + Udp.write(PDU, 3); + if (enableRtuOverTcp) { + Udp.write(lowByte(crc)); // send CRC, low byte first + Udp.write(highByte(crc)); + } + Udp.endPacket(); + ethTxCount += 5; + if (!enableRtuOverTcp) ethTxCount += 4; + } else { + EthernetClient client = EthernetClient(queueHeaders.first().clientNum); + // make sure that this is really our socket + if (client.localPort() == tcpPort && (client.status() == SnSR::ESTABLISHED || client.status() == SnSR::CLOSE_WAIT)) { + if (!enableRtuOverTcp) { + client.write(MBAP, 6); + } + client.write(PDU, 3); + if (enableRtuOverTcp) { + client.write(lowByte(crc)); // send CRC, low byte first + client.write(highByte(crc)); + } + ethTxCount += 5; + if (!enableRtuOverTcp) ethTxCount += 4; + } + } + deleteRequest(); + } // if (queueRetries.first() >= MAX_RETRY) + serialState = MBIDLE; + modbusBusy = false; + } // if (requestTimeout.isOver() && expectingData == true) +} + +bool checkCRC(byte buf[], int len) +{ + crc = 0xFFFF; + for (byte i = 0; i < len - 2; i++) { + calculateCRC(buf[i]); + } + if (highByte(crc) == buf[len - 1] && lowByte(crc) == buf[len - 2]) { + #ifdef DEBUG + debugSerial<<"CRC ok "<>= 1; // Shift right and XOR 0xA001 + crc ^= 0xA001; + } + else // Else LSB is not set + crc >>= 1; // Just shift right + } + // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes) +} + +void ipmodbusLoop() +{ + recvUdp(); + recvTcp(); + processRequests(); + sendSerial(); + recvSerial(); +} + + +void setupIpmodbus(){ + + modbusServer = EthernetServer(tcpPort); + Udp.begin(udpPort); + modbusServer.begin(); + + // Calculate Modbus RTU character timeout and frame delay + byte bits = // number of bits per character (11 in default Modbus RTU settings) + 1 + // start bit + (((MODBUS_DIMMER_PARAM & 0x06) >> 1) + 5) + // data bits + (((MODBUS_DIMMER_PARAM & 0x08) >> 3) + 1); // stop bits + if (((MODBUS_DIMMER_PARAM & 0x30) >> 4) > 1) bits += 1; // parity bit (if present) + //bits = 11; + + int T = ((unsigned long)bits * 1000000UL) / MODBUS_SERIAL_BAUD; // time to send 1 character over serial in microseconds + if (MODBUS_SERIAL_BAUD <= 19200) { + charTimeout = 1.5 * T; // inter-character time-out should be 1,5T + frameDelay = 3.5 * T; // inter-frame delay should be 3,5T + } + else { + charTimeout = 750; + frameDelay = 1750; + } + + //debugSerial<<"Char TO="<begin(); #endif + + #ifdef IPMODBUS + setupIpmodbus(); + #endif initializedListeners = true; } lanStatus = LIBS_INITIALIZED; @@ -2068,7 +2072,7 @@ void setup_main() { #else pinMode(TXEnablePin, OUTPUT); #endif - modbusSerial.begin(MODBUS_SERIAL_BAUD); + modbusSerial.begin(MODBUS_SERIAL_BAUD,dimPar); node.idle(&modbusIdle); node.preTransmission(preTransmission); node.postTransmission(postTransmission); @@ -2273,7 +2277,6 @@ infoSerial< + +boolean MicroTimer::isOver() { + if ((unsigned long)(micros() - timestampLastHitMs) > sleepTimeMs) { + return true; + } + return false; +} + +void MicroTimer::sleep(unsigned long sleepTimeMs) { + this->sleepTimeMs = sleepTimeMs; + timestampLastHitMs = micros(); +} + +boolean Timer::isOver() { + if ((unsigned long)(millis() - timestampLastHitMs) > sleepTimeMs) { + return true; + } + return false; +} + +void Timer::sleep(unsigned long sleepTimeMs) { + this->sleepTimeMs = sleepTimeMs; + timestampLastHitMs = millis(); +} \ No newline at end of file diff --git a/lighthub/microtimer.h b/lighthub/microtimer.h new file mode 100644 index 0000000..5ca12eb --- /dev/null +++ b/lighthub/microtimer.h @@ -0,0 +1,20 @@ +#pragma once +#include +class MicroTimer { + private: + unsigned long timestampLastHitMs; + unsigned long sleepTimeMs; + public: + boolean isOver(); + void sleep(unsigned long sleepTimeMs); +}; + + +class Timer { + private: + unsigned long timestampLastHitMs; + unsigned long sleepTimeMs; + public: + boolean isOver(); + void sleep(unsigned long sleepTimeMs); +}; diff --git a/platformio.ini b/platformio.ini index 3979334..f7f425a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -711,6 +711,7 @@ lib_deps = br3ttb/PID@^1.2.1 ArduinoMDNS https://github.com/khoih-prog/TimerInterrupt_Generic.git + https://github.com/rlogiacco/CircularBuffer monitor_speed = 115200