CAN-bus to ModbusTCP bridge

[FIMC]

Hello all, I am new to the Forum and to the Teensy devices.

I am working on a CAN-ModbusTCP bridge, with a Teensy 4.1 and native PHY Ethernet.

Currently, I have this project working good with an Arduino Mega 2560, Ethernet shield W5500 and CAN shield MCP2515. Everything works and I have a cycle time of about 12 ms, which is good but I would like to improve.

I have ported the code to Teensy, using the libraries QNEthernet and FlexCAN_T4. The Modbus library is the same official one for Arduino "ArduinoModbus".
On the CAN side, it works flawless. Fast and reliable, using mailboxes and filtering.
BUT the ModbusTCP side, I have configured it as ModbusTCP server, and another device communicates as a client to it.

Each time there is a package received (especially when the external device writes holding registers), it delays a lot, making the whole thing unusable.
I have tried both NativeEthernet and QNEthernet libraries. The latter works better but still delays the communication.

This is the main part of the code. Please note MTCP_zz, MTCP_xx and so on are all very similar.

#include <SPI.h>
#include <NativeEthernet.h>
#include <ArduinoRS485.h> // ArduinoModbus depends on the ArduinoRS485 library
#include <ArduinoModbus.h>

#include <FlexCAN_T4.h>
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> Can0;
CAN_message_t msgr;

#define NUM_RX_MAILBOXES 11
#define NUM_TX_MAILBOXES 3

#define NUMB 10

int16_t  pv[NUMB];
bool pe[NUMB];
bool pp[NUMB];
bool pa[NUMB];

uint8_t mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(10,0,0,20); //bridge IP

EthernetServer ethServer(502);

ModbusTCPServer modbusTCPServer;

unsigned long previousMillis = 0;
const long interval = 1000;

uint32_t tcicloprev;

const int ledPin = 13;

void setup() {
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);
  
  Serial.begin(115200);

  Ethernet.begin(mac, ip);
  
  // start the server
  ethServer.begin();

  if (!modbusTCPServer.begin()) {
    while (1);
  }

  modbusTCPServer.configureCoils(0x00, 64);
  modbusTCPServer.configureHoldingRegisters(0x00, 256);

  Can0.begin();
  Can0.setBaudRate(250000);

  Can0.setMaxMB(NUM_TX_MAILBOXES + NUM_RX_MAILBOXES);
  
  for (int i = 0; i<NUM_RX_MAILBOXES; i++){
    Can0.setMB((FLEXCAN_MAILBOX)i,RX,STD);
  }
  for (int i = NUM_RX_MAILBOXES; i<(NUM_TX_MAILBOXES + NUM_RX_MAILBOXES); i++){
    Can0.setMB((FLEXCAN_MAILBOX)i,TX,STD);
  }

  Can0.setMBFilter(REJECT_ALL);
  Can0.enableMBInterrupts();
  
  Can0.onReceive(MB0,canSniff);
  Can0.onReceive(MB1,canSniff);
  Can0.onReceive(MB2,canSniff);
  Can0.onReceive(MB3,canSniff);
  Can0.onReceive(MB4,canSniff);
  Can0.onReceive(MB5,canSniff);
  Can0.onReceive(MB6,canSniff);
  Can0.onReceive(MB7,canSniff);
  Can0.onReceive(MB8,canSniff);
  Can0.onReceive(MB9,canSniff);
  Can0.onReceive(MB10,canSniff);  
  Can0.setMBFilter(MB0,10);
  Can0.setMBFilter(MB1,20);
  Can0.setMBFilter(MB2,30);
  Can0.setMBFilter(MB3,40);
  Can0.setMBFilter(MB4,50);
  Can0.setMBFilter(MB5,60);
  Can0.setMBFilter(MB6,70);
  Can0.setMBFilter(MB7,80);
  Can0.setMBFilter(MB8,90);
  Can0.setMBFilter(MB9,100);
  Can0.setMBFilter(MB10,110);

  Can0.mailboxStatus();

}

void canSniff(const CAN_message_t &msg) {
  Serial.print("MB "); Serial.print(msg.mb);
  Serial.print("  OVERRUN: "); Serial.print(msg.flags.overrun);
  Serial.print("  LEN: "); Serial.print(msg.len);
  Serial.print(" EXT: "); Serial.print(msg.flags.extended);
  Serial.print(" TS: "); Serial.print(msg.timestamp);
  Serial.print(" ID: "); Serial.print(msg.id, HEX);
  Serial.print(" Buffer: ");
  for ( uint8_t i = 0; i < msg.len; i++ ) {
    Serial.print(msg.buf[i], HEX); Serial.print(" ");
  } 
  Serial.println();

  for (uint8_t i=1; i<NUMB+1; i++){
    
    if (msg.id == (10+(i*10))){
      if (msg.len == 2){
        pp[i] = msg.buf[1];
        pa[i] = msg.buf[0];
      }
      if (msg.len == 3){
        pv[i] = (msg.buf[0]*256) + msg.buf[1];
   
        if (msg.buf[2]==0x20) { pe[i] = true;}
        else { pe[i] = false;}
      }
    }
  }
}

void loop() {

  EthernetClient client = ethServer.available();

  if (client) {
    modbusTCPServer.accept(client);
    while (client.connected()) {
      // poll for Modbus TCP requests, while client connected
      modbusTCPServer.poll();
    
      Can0.events();
      MTCP_zz();
      //MTCP_xx();
      //MTCP_yy();
      //MTCP_tt();
    }//END WHILE
    client.stop();
  }//END MODBUS

}

void myCallback() {
  Serial.println("FEED THE DOG SOON, OR RESET!");
}



void MTCP_zz(){

  uint16_t zz = 0;

  CAN_message_t msg;
  
  msg.buf[0]=25;
   
  zz = modbusTCPServer.holdingRegisterRead(0x04);
  if(zz != 0){
    for (int8_t i = 0; i<15; i++){
      
      if (bitRead(zz,i) == 1)
      {
        msg.id = 0x101+(i*10);
        Can0.write(MB12,msg);        
        bitClear(zz,i);
      }
    }
      modbusTCPServer.holdingRegisterWrite(0x04, zz);
  }
}

[BriComp]

#include <SPI.h>
#include <NativeEthernet.h>
#include <ArduinoRS485.h> // ArduinoModbus depends on the ArduinoRS485 library
#include <ArduinoModbus.h>

#include <Crc16.h>
//Crc 16 library (Modbus)
Crc16 crc(true, true, 0x8005, 0xffff, 0x0000, 0x8000, 0xffff);

#include <FlexCAN_T4.h>
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> Can0;
CAN_message_t msgr;

#define NUM_RX_MAILBOXES 11
#define NUM_TX_MAILBOXES 3

#define NUMB 10

int16_t pv[NUMB];
bool pe[NUMB];
bool pp[NUMB];
bool pa[NUMB];

uint8_t mac[] = {
0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(10, 0, 0, 20); //bridge IP

EthernetServer ethServer(502);

ModbusTCPServer modbusTCPServer;

unsigned long previousMillis = 0;
const long interval = 1000;

uint32_t tcicloprev;

const int ledPin = 13;

void setup() {
	pinMode(ledPin, OUTPUT);
	digitalWrite(ledPin, LOW);

	Serial.begin(115200);

	Ethernet.begin(mac, ip);

	// start the server
	ethServer.begin();

	if (!modbusTCPServer.begin()) {
		while (1);
	}

	modbusTCPServer.configureCoils(0x00, 64);
	modbusTCPServer.configureHoldingRegisters(0x00, 256);

	Can0.begin();
	Can0.setBaudRate(250000);

	Can0.setMaxMB(NUM_TX_MAILBOXES + NUM_RX_MAILBOXES);

	for (int i = 0; i < NUM_RX_MAILBOXES; i++) {
		Can0.setMB((FLEXCAN_MAILBOX)i, RX, STD);
	}
	for (int i = NUM_RX_MAILBOXES; i < (NUM_TX_MAILBOXES + NUM_RX_MAILBOXES); i++) {
		Can0.setMB((FLEXCAN_MAILBOX)i, TX, STD);
	}

	Can0.setMBFilter(REJECT_ALL);
	Can0.enableMBInterrupts();

	Can0.onReceive(MB0, canSniff);
	Can0.onReceive(MB1, canSniff);
	Can0.onReceive(MB2, canSniff);
	Can0.onReceive(MB3, canSniff);
	Can0.onReceive(MB4, canSniff);
	Can0.onReceive(MB5, canSniff);
	Can0.onReceive(MB6, canSniff);
	Can0.onReceive(MB7, canSniff);
	Can0.onReceive(MB8, canSniff);
	Can0.onReceive(MB9, canSniff);
	Can0.onReceive(MB10, canSniff);
	Can0.setMBFilter(MB0, 10);
	Can0.setMBFilter(MB1, 20);
	Can0.setMBFilter(MB2, 30);
	Can0.setMBFilter(MB3, 40);
	Can0.setMBFilter(MB4, 50);
	Can0.setMBFilter(MB5, 60);
	Can0.setMBFilter(MB6, 70);
	Can0.setMBFilter(MB7, 80);
	Can0.setMBFilter(MB8, 90);
	Can0.setMBFilter(MB9, 100);
	Can0.setMBFilter(MB10, 110);

	Can0.mailboxStatus();

}

void canSniff(const CAN_message_t& msg) {
	Serial.print("MB "); Serial.print(msg.mb);
	Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun);
	Serial.print(" LEN: "); Serial.print(msg.len);
	Serial.print(" EXT: "); Serial.print(msg.flags.extended);
	Serial.print(" TS: "); Serial.print(msg.timestamp);
	Serial.print(" ID: "); Serial.print(msg.id, HEX);
	Serial.print(" Buffer: ");
	for (uint8_t i = 0; i < msg.len; i++) {
		Serial.print(msg.buf[i], HEX); Serial.print(" ");
	}
	Serial.println();

	for (uint8_t i = 1; i < NUMB + 1; i++) {

		if (msg.id == (10 + (i * 10))) {
			if (msg.len == 2) {
				pp[i] = msg.buf[1];
				pa[i] = msg.buf[0];
			}
			if (msg.len == 3) {
				pv[i] = (msg.buf[0] * 256) + msg.buf[1];

				if (msg.buf[2] == 0x20) { pe[i] = true; }
				else { pe[i] = false; }
			}
		}
	}
}

void loop() {

	EthernetClient client = ethServer.available();

	if (client) {
		modbusTCPServer.accept(client);
		while (client.connected()) {
			// poll for Modbus TCP requests, while client connected
			modbusTCPServer.poll();

			Can0.events();
			MTCP_zz();
			//MTCP_xx();
			//MTCP_yy();
			//MTCP_tt();
		}//END WHILE
		client.stop();
	}//END MODBUS

}

void myCallback() {
	Serial.println("FEED THE DOG SOON, OR RESET!");
}



void MTCP_zz() {

	uint16_t zz = 0;

	CAN_message_t msg;

	msg.buf[0] = 25;

	zz = modbusTCPServer.holdingRegisterRead(0x04);
	if (zz != 0) {
		for (int8_t i = 0; i < 15; i++) {

			if (bitRead(zz, i) == 1)
			{
				msg.id = 0x101 + (i * 10);
				Can0.write(MB12, msg);
				bitClear(zz, i);
			}
		}
		modbusTCPServer.holdingRegisterWrite(0x04, zz);
	}
}

When you post code in the future can you post it between code tags using the # button.
It makes your code so much easier to read and understand which will make it easier for someone to help you.
I have put your code (now formatted) between code tags above. I am sure you can see what I mean.
PS
Welcome to the Teensy Fold.

[FIMC]

Thank you, fixed!

[shawn]

If you want to eliminate any transmission delays, you can call flush() after sending TCP data when using QNEthernet. It’s possible the ArduinoModbus API needs to be changed to support this. Without calling flush(), data is buffered for a short interval, anticipating more data that can be sent in the same packet, for efficiency.