|
| 1 | +/* ******************************************************************* |
| 2 | + Modbus RTU functions |
| 3 | + |
| 4 | + sendSerial |
| 5 | + - sends Modbus RTU requests to HW serial port (RS485 interface) |
| 6 | + |
| 7 | + recvSerial |
| 8 | + - receives Modbus RTU replies |
| 9 | + - adjusts headers and forward messages as Modbus TCP/UDP or Modbus RTU over TCP/UDP |
| 10 | + - sends Modbus TCP/UDP error messages in case Modbus RTU response timeouts |
| 11 | + |
| 12 | + checkCRC |
| 13 | + - checks an array and returns true if CRC is OK |
| 14 | + |
| 15 | + calculateCRC |
| 16 | + |
| 17 | + ***************************************************************** */ |
| 18 | + |
| 19 | +int rxNdx = 0; |
| 20 | +int txNdx = 0; |
| 21 | +bool rxErr = false; |
| 22 | + |
| 23 | +MicroTimer rxDelay; |
| 24 | +MicroTimer rxTimeout; |
| 25 | +MicroTimer txDelay; |
| 26 | + |
| 27 | +void sendSerial() { |
| 28 | + if (serialState == SENDING && rxNdx == 0) { // avoid bus collision, only send when we are not receiving data |
| 29 | + if (mySerial.availableForWrite() > 0 && txNdx == 0) { |
| 30 | +#ifdef RS485_CONTROL_PIN |
| 31 | + digitalWrite(RS485_CONTROL_PIN, RS485_TRANSMIT); // Enable RS485 Transmit |
| 32 | +#endif /* RS485_CONTROL_PIN */ |
| 33 | + crc = 0xFFFF; |
| 34 | + mySerial.write(queueHeaders.first().uid); // send uid (address) |
| 35 | + calculateCRC(queueHeaders.first().uid); |
| 36 | + } |
| 37 | + while (mySerial.availableForWrite() > 0 && txNdx < queueHeaders.first().PDUlen) { |
| 38 | + mySerial.write(queuePDUs[txNdx]); // send func and data |
| 39 | + calculateCRC(queuePDUs[txNdx]); |
| 40 | + txNdx++; |
| 41 | + } |
| 42 | + if (mySerial.availableForWrite() > 1 && txNdx == queueHeaders.first().PDUlen) { |
| 43 | + // In Modbus TCP mode we must add CRC (in Modbus RTU over TCP, CRC is already in queuePDUs) |
| 44 | + if (!localConfig.enableRtuOverTcp || queueHeaders.first().clientNum == SCAN_REQUEST) { |
| 45 | + mySerial.write(lowByte(crc)); // send CRC, low byte first |
| 46 | + mySerial.write(highByte(crc)); |
| 47 | + } |
| 48 | + txNdx++; |
| 49 | + } |
| 50 | + if (mySerial.availableForWrite() == SERIAL_TX_BUFFER_SIZE - 1 && txNdx > queueHeaders.first().PDUlen) { |
| 51 | + // wait for last byte (incl. CRC) to be sent from serial Tx buffer |
| 52 | + // this if statement is not very reliable (too fast) |
| 53 | + // Serial.isFlushed() method is needed....see https://github.com/arduino/Arduino/pull/3737 |
| 54 | + txNdx = 0; |
| 55 | + txDelay.sleep(frameDelay); |
| 56 | + serialState = DELAY; |
| 57 | + } |
| 58 | + } else if (serialState == DELAY && txDelay.isOver()) { |
| 59 | + serialTxCount += queueHeaders.first().PDUlen + 1; // in Modbus RTU over TCP, queuePDUs already contains CRC |
| 60 | + if (!localConfig.enableRtuOverTcp) serialTxCount += 2; // in Modbus TCP, add 2 bytes for CRC |
| 61 | +#ifdef RS485_CONTROL_PIN |
| 62 | + digitalWrite(RS485_CONTROL_PIN, RS485_RECEIVE); // Disable RS485 Transmit |
| 63 | +#endif /* RS485_CONTROL_PIN */ |
| 64 | + if (queueHeaders.first().uid == 0x00) { // Modbus broadcast - we do not count attempts and delete immediatelly |
| 65 | + serialState = IDLE; |
| 66 | + deleteRequest(); |
| 67 | + } else { |
| 68 | + serialState = WAITING; |
| 69 | + requestTimeout.sleep(localConfig.serialTimeout); // delays next serial write |
| 70 | + queueRetries.unshift(queueRetries.shift() + 1); |
| 71 | + } |
| 72 | + } |
| 73 | +} |
| 74 | + |
| 75 | +void recvSerial() { |
| 76 | + static byte serialIn[modbusSize]; |
| 77 | + while (mySerial.available() > 0) { |
| 78 | + if (rxTimeout.isOver() && rxNdx != 0) { |
| 79 | + rxErr = true; // character timeout |
| 80 | + } |
| 81 | + if (rxNdx < modbusSize) { |
| 82 | + serialIn[rxNdx] = mySerial.read(); |
| 83 | + rxNdx++; |
| 84 | + } else { |
| 85 | + mySerial.read(); |
| 86 | + rxErr = true; // frame longer than maximum allowed |
| 87 | + } |
| 88 | + rxDelay.sleep(frameDelay); |
| 89 | + rxTimeout.sleep(charTimeout); |
| 90 | + } |
| 91 | + if (rxDelay.isOver() && rxNdx != 0) { |
| 92 | + |
| 93 | + // Process Serial data |
| 94 | + // 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 |
| 95 | + if (!rxErr && checkCRC(serialIn, rxNdx) == true && serialIn[0] == queueHeaders.first().uid && serialState == WAITING) { |
| 96 | + setSlaveResponding(serialIn[0], true); // flag slave as responding |
| 97 | + byte MBAP[] = { queueHeaders.first().tid[0], queueHeaders.first().tid[1], 0x00, 0x00, highByte(rxNdx - 2), lowByte(rxNdx - 2) }; |
| 98 | + if (queueHeaders.first().clientNum == UDP_REQUEST) { |
| 99 | + Udp.beginPacket(queueHeaders.first().remIP, queueHeaders.first().remPort); |
| 100 | + if (localConfig.enableRtuOverTcp) Udp.write(serialIn, rxNdx); |
| 101 | + else { |
| 102 | + Udp.write(MBAP, 6); |
| 103 | + Udp.write(serialIn, rxNdx - 2); //send without CRC |
| 104 | + } |
| 105 | + Udp.endPacket(); |
| 106 | +#ifdef ENABLE_EXTRA_DIAG |
| 107 | + ethTxCount += rxNdx; |
| 108 | + if (!localConfig.enableRtuOverTcp) ethTxCount += 4; |
| 109 | +#endif /* ENABLE_EXTRA_DIAG */ |
| 110 | + } else if (queueHeaders.first().clientNum != SCAN_REQUEST) { |
| 111 | + EthernetClient client = EthernetClient(queueHeaders.first().clientNum); |
| 112 | + // make sure that this is really our socket |
| 113 | + if (client.localPort() == localConfig.tcpPort && (client.status() == SnSR::ESTABLISHED || client.status() == SnSR::CLOSE_WAIT)) { |
| 114 | + if (localConfig.enableRtuOverTcp) client.write(serialIn, rxNdx); |
| 115 | + else { |
| 116 | + client.write(MBAP, 6); |
| 117 | + client.write(serialIn, rxNdx - 2); //send without CRC |
| 118 | + } |
| 119 | +#ifdef ENABLE_EXTRA_DIAG |
| 120 | + ethTxCount += rxNdx; |
| 121 | + if (!localConfig.enableRtuOverTcp) ethTxCount += 4; |
| 122 | +#endif /* ENABLE_EXTRA_DIAG */ |
| 123 | + } |
| 124 | + } |
| 125 | + deleteRequest(); |
| 126 | + serialState = IDLE; |
| 127 | + } |
| 128 | + serialRxCount += rxNdx; |
| 129 | + rxNdx = 0; |
| 130 | + rxErr = false; |
| 131 | + } |
| 132 | + |
| 133 | + // Deal with Serial timeouts (i.e. Modbus RTU timeouts) |
| 134 | + if (serialState == WAITING && requestTimeout.isOver()) { |
| 135 | + setSlaveResponding(queueHeaders.first().uid, false); // flag slave as nonresponding |
| 136 | + if (queueRetries.first() >= localConfig.serialAttempts) { |
| 137 | + // send modbus error 11 (Gateway Target Device Failed to Respond) - usually means that target device (address) is not present |
| 138 | + byte MBAP[] = { queueHeaders.first().tid[0], queueHeaders.first().tid[1], 0x00, 0x00, 0x00, 0x03 }; |
| 139 | + byte PDU[] = { queueHeaders.first().uid, (byte)(queuePDUs[0] + 0x80), 0x0B }; |
| 140 | + crc = 0xFFFF; |
| 141 | + for (byte i = 0; i < sizeof(PDU); i++) { |
| 142 | + calculateCRC(PDU[i]); |
| 143 | + } |
| 144 | + if (queueHeaders.first().clientNum == UDP_REQUEST) { |
| 145 | + Udp.beginPacket(queueHeaders.first().remIP, queueHeaders.first().remPort); |
| 146 | + if (!localConfig.enableRtuOverTcp) { |
| 147 | + Udp.write(MBAP, 6); |
| 148 | + } |
| 149 | + Udp.write(PDU, 3); |
| 150 | + if (localConfig.enableRtuOverTcp) { |
| 151 | + Udp.write(lowByte(crc)); // send CRC, low byte first |
| 152 | + Udp.write(highByte(crc)); |
| 153 | + } |
| 154 | + Udp.endPacket(); |
| 155 | +#ifdef ENABLE_EXTRA_DIAG |
| 156 | + ethTxCount += 5; |
| 157 | + if (!localConfig.enableRtuOverTcp) ethTxCount += 4; |
| 158 | +#endif /* ENABLE_EXTRA_DIAG */ |
| 159 | + } else { |
| 160 | + EthernetClient client = EthernetClient(queueHeaders.first().clientNum); |
| 161 | + // make sure that this is really our socket |
| 162 | + if (client.localPort() == localConfig.tcpPort && (client.status() == SnSR::ESTABLISHED || client.status() == SnSR::CLOSE_WAIT)) { |
| 163 | + if (!localConfig.enableRtuOverTcp) { |
| 164 | + client.write(MBAP, 6); |
| 165 | + } |
| 166 | + client.write(PDU, 3); |
| 167 | + if (localConfig.enableRtuOverTcp) { |
| 168 | + client.write(lowByte(crc)); // send CRC, low byte first |
| 169 | + client.write(highByte(crc)); |
| 170 | + } |
| 171 | +#ifdef ENABLE_EXTRA_DIAG |
| 172 | + ethTxCount += 5; |
| 173 | + if (!localConfig.enableRtuOverTcp) ethTxCount += 4; |
| 174 | +#endif /* ENABLE_EXTRA_DIAG */ |
| 175 | + } |
| 176 | + } |
| 177 | + deleteRequest(); |
| 178 | + } // if (queueRetries.first() >= MAX_RETRY) |
| 179 | + serialState = IDLE; |
| 180 | + } // if (requestTimeout.isOver() && expectingData == true) |
| 181 | +} |
| 182 | + |
| 183 | +bool checkCRC(byte buf[], int len) { |
| 184 | + crc = 0xFFFF; |
| 185 | + for (byte i = 0; i < len - 2; i++) { |
| 186 | + calculateCRC(buf[i]); |
| 187 | + } |
| 188 | + if (highByte(crc) == buf[len - 1] && lowByte(crc) == buf[len - 2]) { |
| 189 | + return true; |
| 190 | + } else { |
| 191 | + return false; |
| 192 | + } |
| 193 | +} |
| 194 | + |
| 195 | +void calculateCRC(byte b) { |
| 196 | + crc ^= (uint16_t)b; // XOR byte into least sig. byte of crc |
| 197 | + for (byte i = 8; i != 0; i--) { // Loop over each bit |
| 198 | + if ((crc & 0x0001) != 0) { // If the LSB is set |
| 199 | + crc >>= 1; // Shift right and XOR 0xA001 |
| 200 | + crc ^= 0xA001; |
| 201 | + } else // Else LSB is not set |
| 202 | + crc >>= 1; // Just shift right |
| 203 | + } |
| 204 | + // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes) |
| 205 | +} |
0 commit comments