Skip to content

Commit b2fab75

Browse files
budulinekbudulinek
committed
Simplify queue
1 parent 856a76d commit b2fab75

File tree

3 files changed

+65
-280
lines changed

3 files changed

+65
-280
lines changed

‎arduino-modbus-rtu-tcp-gateway/02-modbus-tcp.ino

Lines changed: 32 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@
2626
2727
***************************************************************** */
2828

29+
#define ADDRESS_POS (6 * !localConfig.enableRtuOverTcp) // position of slave address in the TCP/UDP message (0 for Modbus RTU over TCP/UDP and 6 for Modbus RTU over TCP/UDP)
30+
2931
// bool arrays for storing Modbus RTU status (responging or not responding). Array index corresponds to slave address.
3032
uint8_t responding[(maxSlaves + 1 + 7) / 8];
3133
uint8_t error[(maxSlaves + 1 + 7) / 8];
@@ -44,38 +46,33 @@ typedef struct {
4446
CircularBuffer<header, reqQueueCount> queueHeaders; // queue of requests' headers and metadata (MBAP transaction ID, MBAP unit ID, PDU length, remIP, remPort, TCP client)
4547
CircularBuffer<byte, reqQueueSize> queuePDUs; // queue of PDU data (function code, data)
4648
CircularBuffer<byte, reqQueueCount> queueRetries; // queue of retry counters
47-
byte pduStart; // first byte of Protocol Data Unit (i.e. Function code)
48-
4949

5050
void recvUdp() {
51-
unsigned int packetSize = Udp.parsePacket();
52-
if (packetSize) {
51+
unsigned int msgLength = Udp.parsePacket();
52+
if (msgLength) {
5353
#ifdef ENABLE_EXTRA_DIAG
54-
ethRxCount += packetSize;
55-
#endif /* ENABLE_EXTRA_DIAG */
54+
ethRxCount += msgLength;
55+
#endif /* ENABLE_EXTRA_DIAG */
5656
byte inBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
5757
// Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address)..... no CRC
5858
// Modbus RTU frame: [0] address.....[n-1][n] CRC
5959
Udp.read(inBuffer, sizeof(inBuffer));
6060
Udp.flush();
61-
if (localConfig.enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address)
62-
else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address)
63-
64-
byte errorCode = checkRequest(inBuffer, packetSize, (IPAddress)Udp.remoteIP(), Udp.remotePort(), UDP_REQUEST);
61+
byte errorCode = checkRequest(inBuffer, msgLength, (IPAddress)Udp.remoteIP(), Udp.remotePort(), UDP_REQUEST);
6562
if (errorCode) {
6663
// send back message with error code
6764
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
6865
if (!localConfig.enableRtuOverTcp) {
6966
Udp.write(inBuffer, 5);
7067
Udp.write(0x03);
7168
}
72-
Udp.write(inBuffer[pduStart - 1]); // address
73-
Udp.write(inBuffer[pduStart] + 0x80); // function + 0x80
69+
Udp.write(inBuffer[ADDRESS_POS]); // address
70+
Udp.write(inBuffer[ADDRESS_POS + 1] + 0x80); // function + 0x80
7471
Udp.write(errorCode);
7572
if (localConfig.enableRtuOverTcp) {
7673
crc = 0xFFFF;
77-
calculateCRC(inBuffer[pduStart - 1]);
78-
calculateCRC(inBuffer[pduStart] + 0x80);
74+
calculateCRC(inBuffer[ADDRESS_POS]);
75+
calculateCRC(inBuffer[ADDRESS_POS + 1] + 0x80);
7976
calculateCRC(errorCode);
8077
Udp.write(lowByte(crc)); // send CRC, low byte first
8178
Udp.write(highByte(crc));
@@ -92,33 +89,29 @@ void recvUdp() {
9289
void recvTcp() {
9390
EthernetClient client = modbusServer.available();
9491
if (client) {
95-
unsigned int packetSize = client.available();
92+
unsigned int msgLength = client.available();
9693
#ifdef ENABLE_EXTRA_DIAG
97-
ethRxCount += packetSize;
98-
#endif /* ENABLE_EXTRA_DIAG */
94+
ethRxCount += msgLength;
95+
#endif /* ENABLE_EXTRA_DIAG */
9996
byte inBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
10097
// Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address).....
10198
// Modbus RTU frame: [0] address.....
10299
client.read(inBuffer, sizeof(inBuffer));
103100
client.flush();
104-
105-
if (localConfig.enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address)
106-
else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address)
107-
108-
byte errorCode = checkRequest(inBuffer, packetSize, (IPAddress){}, 0, client.getSocketNumber());
101+
byte errorCode = checkRequest(inBuffer, msgLength, (IPAddress){}, 0, client.getSocketNumber());
109102
if (errorCode) {
110103
// send back message with error code
111104
if (!localConfig.enableRtuOverTcp) {
112105
client.write(inBuffer, 5);
113106
client.write(0x03);
114107
}
115-
client.write(inBuffer[pduStart - 1]); // address
116-
client.write(inBuffer[pduStart] + 0x80); // function + 0x80
108+
client.write(inBuffer[ADDRESS_POS]); // address
109+
client.write(inBuffer[ADDRESS_POS + 1] + 0x80); // function + 0x80
117110
client.write(errorCode);
118111
if (localConfig.enableRtuOverTcp) {
119112
crc = 0xFFFF;
120-
calculateCRC(inBuffer[pduStart - 1]);
121-
calculateCRC(inBuffer[pduStart] + 0x80);
113+
calculateCRC(inBuffer[ADDRESS_POS]);
114+
calculateCRC(inBuffer[ADDRESS_POS + 1] + 0x80);
122115
calculateCRC(errorCode);
123116
client.write(lowByte(crc)); // send CRC, low byte first
124117
client.write(highByte(crc));
@@ -133,10 +126,11 @@ void recvTcp() {
133126

134127
void processRequests() {
135128
// Insert scan request into queue
136-
if (scanCounter != 0 && queueHeaders.available() > 1 && queuePDUs.available() > 1) {
129+
if (scanCounter != 0 && queueHeaders.available() > 1 && queuePDUs.available() > sizeof(scanCommand) + 1) {
137130
// Store scan request in request queue
138-
queueHeaders.push(header{ { 0x00, 0x00 }, scanCounter, sizeof(scanCommand), {}, 0, SCAN_REQUEST });
131+
queueHeaders.push(header{ { 0x00, 0x00 }, scanCounter, sizeof(scanCommand) + 1, {}, 0, SCAN_REQUEST });
139132
queueRetries.push(localConfig.serialAttempts - 1); // scan requests are only sent once, so set "queueRetries" to one attempt below limit
133+
queuePDUs.push(scanCounter); // address of the scanned slave
140134
for (byte i = 0; i < sizeof(scanCommand); i++) {
141135
queuePDUs.push(scanCommand[i]);
142136
}
@@ -160,29 +154,30 @@ void processRequests() {
160154
}
161155
}
162156

163-
byte checkRequest(byte inBuffer[], unsigned int packetSize, IPAddress remoteIP, unsigned int remotePort, byte clientNum) {
157+
byte checkRequest(const byte inBuffer[], unsigned int msgLength, const IPAddress remoteIP, const unsigned int remotePort, const byte clientNum) {
164158
byte address;
165159
if (localConfig.enableRtuOverTcp) address = inBuffer[0];
166160
else address = inBuffer[6];
167161
if (localConfig.enableRtuOverTcp) { // check CRC for Modbus RTU over TCP/UDP
168-
if (checkCRC(inBuffer, packetSize) == false) {
162+
if (checkCRC(inBuffer, msgLength) == false) {
169163
return 0; // reject: do nothing and return no error code
170164
}
171165
} else { // check MBAP header structure for Modbus TCP/UDP
172-
if (inBuffer[2] != 0x00 || inBuffer[3] != 0x00 || inBuffer[4] != 0x00 || inBuffer[5] != packetSize - 6) {
166+
if (inBuffer[2] != 0x00 || inBuffer[3] != 0x00 || inBuffer[4] != 0x00 || inBuffer[5] != msgLength - 6) {
173167
return 0; // reject: do nothing and return no error code
174168
}
175169
}
170+
msgLength = msgLength - ADDRESS_POS - (2 * localConfig.enableRtuOverTcp); // in Modbus RTU over TCP/UDP do not store CRC
176171
// check if we have space in request queue
177-
if (queueHeaders.available() < 1 || (localConfig.enableRtuOverTcp && queuePDUs.available() < packetSize - 1) || (!localConfig.enableRtuOverTcp && queuePDUs.available() < packetSize - 7)) {
172+
if (queueHeaders.available() < 1 || queuePDUs.available() < msgLength) {
178173
return 0x06; // return modbus error 6 (Slave Device Busy) - try again later
179174
}
180-
// al checkes passed OK, we can store the incoming data in request queue
181-
// 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
182-
queueHeaders.push(header{ { inBuffer[0], inBuffer[1] }, inBuffer[pduStart - 1], (byte)(packetSize - pduStart), (IPAddress)remoteIP, (unsigned int)remotePort, (byte)clientNum });
175+
// all checkes passed OK, we can store the incoming data in request queue
176+
// Store in request queue: 2 bytes MBAP Transaction ID (ignored in Modbus RTU over TCP); MBAP Unit ID (address); message length; remote IP; remote port; TCP client Number (socket) - 0xFF for UDP
177+
queueHeaders.push(header{ { inBuffer[0], inBuffer[1] }, inBuffer[ADDRESS_POS], (byte)msgLength, (IPAddress)remoteIP, (unsigned int)remotePort, (byte)clientNum });
183178
queueRetries.push(0);
184-
for (byte i = 0; i < packetSize - pduStart; i++) {
185-
queuePDUs.push(inBuffer[i + pduStart]);
179+
for (byte i = 0; i < msgLength; i++) {
180+
queuePDUs.push(inBuffer[i + ADDRESS_POS]);
186181
}
187182
return 0;
188183
}

‎arduino-modbus-rtu-tcp-gateway/03-modbus-rtu.ino

Lines changed: 33 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -32,20 +32,15 @@ void sendSerial() {
3232
digitalWrite(RS485_CONTROL_PIN, RS485_TRANSMIT); // Enable RS485 Transmit
3333
#endif /* RS485_CONTROL_PIN */
3434
crc = 0xFFFF;
35-
mySerial.write(myHeader.uid); // send uid (address)
36-
calculateCRC(myHeader.uid);
3735
}
3836
while (mySerial.availableForWrite() > 0 && txNdx < myHeader.PDUlen) {
39-
mySerial.write(queuePDUs[txNdx]); // send func and data
37+
mySerial.write(queuePDUs[txNdx]);
4038
calculateCRC(queuePDUs[txNdx]);
4139
txNdx++;
4240
}
4341
if (mySerial.availableForWrite() > 1 && txNdx == myHeader.PDUlen) {
44-
// In Modbus TCP mode we must add CRC (in Modbus RTU over TCP, CRC is already in queuePDUs)
45-
if (!localConfig.enableRtuOverTcp || myHeader.clientNum == SCAN_REQUEST) {
46-
mySerial.write(lowByte(crc)); // send CRC, low byte first
47-
mySerial.write(highByte(crc));
48-
}
42+
mySerial.write(lowByte(crc)); // send CRC, low byte first
43+
mySerial.write(highByte(crc));
4944
txNdx++;
5045
}
5146
if (mySerial.availableForWrite() == SERIAL_TX_BUFFER_SIZE - 1 && txNdx > myHeader.PDUlen) {
@@ -58,12 +53,12 @@ void sendSerial() {
5853
}
5954
} else if (serialState == DELAY && txDelay.isOver()) {
6055
header myHeader = queueHeaders.first();
61-
serialTxCount += myHeader.PDUlen + 1; // in Modbus RTU over TCP, queuePDUs already contains CRC
62-
if (!localConfig.enableRtuOverTcp) serialTxCount += 2; // in Modbus TCP, add 2 bytes for CRC
56+
serialTxCount += myHeader.PDUlen;
57+
serialTxCount += 2;
6358
#ifdef RS485_CONTROL_PIN
6459
digitalWrite(RS485_CONTROL_PIN, RS485_RECEIVE); // Disable RS485 Transmit
6560
#endif /* RS485_CONTROL_PIN */
66-
if (myHeader.uid == 0x00) { // Modbus broadcast - we do not count attempts and delete immediatelly
61+
if (queuePDUs[0] == 0x00) { // Modbus broadcast - we do not count attempts and delete immediatelly
6762
serialState = IDLE;
6863
deleteRequest();
6964
} else {
@@ -95,7 +90,7 @@ void recvSerial() {
9590
// Process Serial data
9691
// 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
9792
header myHeader = queueHeaders.first();
98-
if (!rxErr && checkCRC(serialIn, rxNdx) == true && serialIn[0] == myHeader.uid && serialState == WAITING) {
93+
if (!rxErr && checkCRC(serialIn, rxNdx) == true && serialIn[0] == queuePDUs[0] && serialState == WAITING) {
9994
setSlaveStatus(serialIn[0], responding, true); // flag slave as responding
10095
setSlaveStatus(serialIn[0], error, false); // flag slave as without error
10196
byte MBAP[] = { myHeader.tid[0], myHeader.tid[1], 0x00, 0x00, highByte(rxNdx - 2), lowByte(rxNdx - 2) };
@@ -136,12 +131,12 @@ void recvSerial() {
136131
// Deal with Serial timeouts (i.e. Modbus RTU timeouts)
137132
if (serialState == WAITING && requestTimeout.isOver()) {
138133
header myHeader = queueHeaders.first();
139-
setSlaveStatus(myHeader.uid, responding, false); // flag slave as nonresponding
140-
if (myHeader.clientNum != SCAN_REQUEST) setSlaveStatus(myHeader.uid, error, true); // flag slave as error
134+
setSlaveStatus(queuePDUs[0], responding, false); // flag slave as nonresponding
135+
if (myHeader.clientNum != SCAN_REQUEST) setSlaveStatus(queuePDUs[0], error, true); // flag slave as error
141136
if (queueRetries.first() >= localConfig.serialAttempts) {
142137
// send modbus error 11 (Gateway Target Device Failed to Respond) - usually means that target device (address) is not present
143138
byte MBAP[] = { myHeader.tid[0], myHeader.tid[1], 0x00, 0x00, 0x00, 0x03 };
144-
byte PDU[] = { myHeader.uid, (byte)(queuePDUs[0] + 0x80), 0x0B };
139+
byte PDU[] = { queuePDUs[0], (byte)(queuePDUs[1] + 0x80), 0x0B };
145140
crc = 0xFFFF;
146141
for (byte i = 0; i < sizeof(PDU); i++) {
147142
calculateCRC(PDU[i]);
@@ -151,37 +146,37 @@ void recvSerial() {
151146
break;
152147
case UDP_REQUEST:
153148
Udp.beginPacket(myHeader.remIP, myHeader.remPort);
154-
if (!localConfig.enableRtuOverTcp) {
155-
Udp.write(MBAP, 6);
156-
}
157-
Udp.write(PDU, 3);
158-
if (localConfig.enableRtuOverTcp) {
159-
Udp.write(lowByte(crc)); // send CRC, low byte first
160-
Udp.write(highByte(crc));
161-
}
162-
Udp.endPacket();
163-
#ifdef ENABLE_EXTRA_DIAG
164-
ethTxCount += 5;
165-
if (!localConfig.enableRtuOverTcp) ethTxCount += 4;
166-
#endif /* ENABLE_EXTRA_DIAG */
167-
break;
168-
default: // Ethernet client
169-
{
170-
EthernetClient client = EthernetClient(myHeader.clientNum);
171-
if (client.connected()) {
172149
if (!localConfig.enableRtuOverTcp) {
173-
client.write(MBAP, 6);
150+
Udp.write(MBAP, 6);
174151
}
175-
client.write(PDU, 3);
152+
Udp.write(PDU, 3);
176153
if (localConfig.enableRtuOverTcp) {
177-
client.write(lowByte(crc)); // send CRC, low byte first
178-
client.write(highByte(crc));
154+
Udp.write(lowByte(crc)); // send CRC, low byte first
155+
Udp.write(highByte(crc));
179156
}
157+
Udp.endPacket();
180158
#ifdef ENABLE_EXTRA_DIAG
181159
ethTxCount += 5;
182160
if (!localConfig.enableRtuOverTcp) ethTxCount += 4;
183161
#endif /* ENABLE_EXTRA_DIAG */
184-
}
162+
break;
163+
default: // Ethernet client
164+
{
165+
EthernetClient client = EthernetClient(myHeader.clientNum);
166+
if (client.connected()) {
167+
if (!localConfig.enableRtuOverTcp) {
168+
client.write(MBAP, 6);
169+
}
170+
client.write(PDU, 3);
171+
if (localConfig.enableRtuOverTcp) {
172+
client.write(lowByte(crc)); // send CRC, low byte first
173+
client.write(highByte(crc));
174+
}
175+
#ifdef ENABLE_EXTRA_DIAG
176+
ethTxCount += 5;
177+
if (!localConfig.enableRtuOverTcp) ethTxCount += 4;
178+
#endif /* ENABLE_EXTRA_DIAG */
179+
}
185180
break;
186181
}
187182
}

0 commit comments

Comments
 (0)