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);
  }
}