Incoming CAN data seems extremely slow, the M2 is getting hung up somewhere, but I can't find where


#1

Sorry for the long title, but I’m a bit stuck. I’ve confirmed that I can indeed see the frames I’m interested in and that my filters work as expected, but for some reason the data seems to get “backed up” or is just downright slow. If I look with a proper CAN interface I can see the messages at the correct rate (20ms), but in my code, even when I adjust the filtering to only allow a single 20ms ID through, I get an update rate of about 2Hz, tops. When I enable all frames it drops like a rock. What’s interesting is that the voltage measurement (which is done on board) comes through nice and quick via the UART to BT to RealDash.

Any help would be greatly appreciated. Maybe I’m just not setting something up right.

Thanks!

Code below:
/*
* RS_BT_RealDash V0.1
* Austin R. Bartz
* 10/2/2018
*
* This application decodes signals of interest on the
* Ford Focus RS HS-CAN Bus and sends it via Bluetooth Serial
* to RealDash.
*
* Signals current implemented:
* Accelerator Pedal Position
* Boost Pressure
* Brake Pressure
* Charge Air Temperature
* Clutch Pedal Position
* Engine Coolant Temperature
* Engine Oil Temperature
* Engine Speed
* Engine Torque
* Individual Wheel Speeds
* 3 Axis Accelerations
* Yaw Rate
* PTU Temperature
* Steering Wheel Angle
* RDU Torque Vectoring Torques
* Vehicle Speed
*
*/

unsigned int APP = 0;
unsigned int Boost = 0;
unsigned int BPP = 0;
unsigned int CAT = 0;
unsigned int CPP = 0;
unsigned int ECT = 0;
unsigned int EOT = 0;
unsigned int AAT = 0;
unsigned int RPM = 0;
unsigned int ETq = 0;
unsigned int WTq = 0;
unsigned int LatAcc = 0;
unsigned int LongAcc = 0;
unsigned int VertAcc = 0;
unsigned int Yaw = 0;
unsigned int FLWhlSpd = 0;
unsigned int FRWhlSpd = 0;
unsigned int RLWhlSpd = 0;
unsigned int RRWhlSpd = 0;
unsigned int PTUT = 0;
unsigned int SWAng = 0;
unsigned int RDUReqTqL = 0;
unsigned int RDUActTqL = 0;
unsigned int RDUReqTqR = 0;
unsigned int RDUActTqR = 0;
unsigned int VehSpd = 0;
unsigned int FuelLvl = 0;
unsigned int BattV = 0;

#include "variant.h"
#include <due_can.h>
#include <Debounce.h>
#include <M2_12VIO.h>

unsigned long previousMillis = 0;
bool redled = 1;
bool greenled = 1;

M2_12VIO M2IO;

void setup()
{
  // Initialize Serial
  SerialUSB.begin(115200);
  Serial3.begin(9600);  //  Needs to be sped up in the future, current BT adaptor only functions at 9600.
  // Initialize CAN
  Can0.begin(CAN_BPS_500K);
  Can0.setRXFilter(0, 0x90, 0x7ff, false);
  Can0.setRXFilter(1, 0x90, 0x7ff, false);
  Can0.setRXFilter(2, 0x90, 0x7ff, false);
  Can0.setRXFilter(3, 0x90, 0x7ff, false);
  Can0.setRXFilter(4, 0x90, 0x7ff, false);
  Can0.setRXFilter(5, 0x90, 0x7ff, false);
  Can0.setRXFilter(6, 0x90, 0x7ff, false);
  Can0.setRXFilter(7, 0x90, 0x7ff, false);
  // Actual filters are below, the above is simply for testing to limit it to just a single can ID.
//  Can0.setRXFilter(1, 0x70, 0x7ff, false);
//  Can0.setRXFilter(2, 0x80, 0x787, false);
//  Can0.setRXFilter(3, 0x138, 0x707, false);
//  Can0.setRXFilter(4, 0x213, 0x60C, false);
//  Can0.setRXFilter(5, 0x0, 0x7ff, false);
//  Can0.setRXFilter(6, 0x0, 0x7ff, false);
//  Can0.setRXFilter(7, 0x0, 0x7ff, false);
  // Initialize M2IO
  M2IO.Init_12VIO();
  pinMode(DS2, OUTPUT);
  pinMode(DS7_GREEN, OUTPUT);
}

void loop()
{
  CAN_FRAME incoming;

  if (Can0.rx_avail())
  {
    Can0.read(incoming); 
    switch (incoming.id)
    {
      case 0x10:
        SWAng   = (((incoming.data.byte[4] & 0x80) << 8) + ((incoming.data.byte[6] & 0x7f) << 8) + incoming.data.byte[7]);
        break;
      case 0x70:
        ETq     = (((incoming.data.byte[2] & 0x07) << 8) + incoming.data.byte[3]);
        break;
      case 0x80:
        APP     = (((incoming.data.byte[0] & 0x03) << 8) + incoming.data.byte[1]);
        WTq     = ((incoming.data.byte[5] << 8) + incoming.data.byte[6]);
        break;
      case 0x90:
        RPM     = (((incoming.data.byte[4] & 0x0f) << 8) + incoming.data.byte[5]);
        break;
      case 0xF8:
        EOT     = incoming.data.byte[1];
        Boost   = incoming.data.byte[5];
        PTUT    = incoming.data.byte[7];
        break;
      case 0x138:
        CPP     = (((incoming.data.byte[2] & 0x03) << 8) + incoming.data.byte[3]);
        break;
      case 0x160:
        VehSpd  = ((incoming.data.byte[2] << 8) + incoming.data.byte[3]);
        LongAcc = (((incoming.data.byte[6] & 0x03) << 8) + incoming.data.byte[7]);
        break;
      case 0x180:
        VertAcc = (((incoming.data.byte[0] & 0x03) << 8) + incoming.data.byte[1]);
        LatAcc  = (((incoming.data.byte[2] & 0x03) << 8) + incoming.data.byte[3]);
        Yaw     = (((incoming.data.byte[4] & 0x0f) << 8) + incoming.data.byte[5]);
        break;
      case 0x190:
        FLWhlSpd = (((incoming.data.byte[0] & 0x7f) << 8) + incoming.data.byte[1]);
        FRWhlSpd = (((incoming.data.byte[2] & 0x7f) << 8) + incoming.data.byte[3]);
        RLWhlSpd = (((incoming.data.byte[4] & 0x7f) << 8) + incoming.data.byte[5]);
        RRWhlSpd = (((incoming.data.byte[6] & 0x7f) << 8) + incoming.data.byte[7]);
        break;
      case 0x213:
        BPP       = (((incoming.data.byte[2] & 0x0f) << 8) + incoming.data.byte[3]);
        break;
      case 0x2C0:
        RDUReqTqL = (((incoming.data.byte[0] & 0x0f) << 8) + incoming.data.byte[1]);
        RDUActTqL = (((incoming.data.byte[2] & 0x0f) << 8) + incoming.data.byte[3]);
        RDUReqTqR = (((incoming.data.byte[4] & 0x0f) << 8) + incoming.data.byte[5]);
        RDUActTqR = (((incoming.data.byte[6] & 0x0f) << 8) + incoming.data.byte[7]);
        break;
      case 0x2F0:
        ECT       = (((incoming.data.byte[4] & 0x03) << 8) + incoming.data.byte[5]);
        CAT       = (((incoming.data.byte[6] & 0x03) << 8) + incoming.data.byte[7]);
        break;
      case 0x340:
        AAT       = incoming.data.byte[7];
        break;
      case 0x380:
        FuelLvl   = (((incoming.data.byte[2] & 0x03) << 8) + incoming.data.byte[3]);
        break;
    }
    SendCANFramesToSerial();
    greenled = greenled ^ 1;
    digitalWrite(DS7_GREEN, greenled);
  }
  BattV = M2IO.Supply_Volts();  // This comes through quickly, pretty much real time.  So I know there isn't a bottle neck in the UART/BT.

  // Heartbeat signal in case of no CAN traffic to keep RealDash happy.
  unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= 100)
    {
      previousMillis = currentMillis;
      SendCANFramesToSerial();
      redled = redled ^ 1;
      digitalWrite(DS2, redled);
    }
  
}

void SendCANFramesToSerial()
{
  byte buf[8];

  // build & send CAN frames to RealDash.
  // a CAN frame payload is always 8 bytes containing data in a manner
  // described by the RealDash custom channel description XML file

  buf[0] = ((FLWhlSpd >> 8) & 0xff);
  buf[1] = (FLWhlSpd & 0xff);
  buf[2] = ((FRWhlSpd >> 8) & 0xff);
  buf[3] = (FRWhlSpd & 0xff);
  buf[4] = ((RLWhlSpd >> 8) & 0xff);
  buf[5] = (RLWhlSpd & 0xff);
  buf[6] = ((RRWhlSpd >> 8) & 0xff);
  buf[7] = (RRWhlSpd & 0xff);
  SendCANFrameToSerial(3200, buf);

  buf[0] = ((RDUReqTqL >> 8) & 0xff);
  buf[1] = (RDUReqTqL & 0xff);
  buf[2] = ((RDUActTqL >> 8) & 0xff);
  buf[3] = (RDUActTqL & 0xff);
  buf[4] = ((RDUReqTqR >> 8) & 0xff);
  buf[5] = (RDUReqTqR & 0xff);
  buf[6] = ((RDUActTqR >> 8) & 0xff);
  buf[7] = (RDUActTqR & 0xff);
  SendCANFrameToSerial(3201, buf);

  buf[0] = (AAT & 0xff);
  buf[1] = (EOT & 0xff);
  buf[2] = (PTUT & 0xff);
  buf[3] = ((CAT >> 8) & 0xff);
  buf[4] = (CAT & 0xff);
  buf[5] = ((ECT >> 8) & 0xff);
  buf[6] = (ECT & 0xff);
  buf[7] = (Boost & 0xff);
  SendCANFrameToSerial(3202, buf);

  buf[0] = ((SWAng >> 8) & 0xff);
  buf[1] = (SWAng & 0xff);
  buf[2] = ((BPP >> 8) & 0xff);
  buf[3] = (BPP & 0xff);
  buf[4] = ((APP >> 8) & 0xff);
  buf[5] = (APP & 0xff);
  buf[6] = ((CPP >> 8) & 0xff);
  buf[7] = (CPP & 0xff);
  SendCANFrameToSerial(3203, buf);

  buf[0] = ((RPM >> 8) & 0xff);
  buf[1] = (RPM & 0xff);
  buf[2] = ((ETq >> 8) & 0xff);
  buf[3] = (ETq & 0xff);
  buf[4] = ((WTq >> 8) & 0xff);
  buf[5] = (WTq & 0xff);
  buf[6] = ((VehSpd >> 8) & 0xff);
  buf[7] = (VehSpd & 0xff);
  SendCANFrameToSerial(3204, buf);

  buf[0] = ((Yaw >> 8) & 0xff);
  buf[1] = (Yaw & 0xff);
  buf[2] = ((LatAcc >> 8) & 0xff);
  buf[3] = (LatAcc & 0xff);
  buf[4] = ((LongAcc >> 8) & 0xff);
  buf[5] = (LongAcc & 0xff);
  buf[6] = ((VertAcc >> 8) & 0xff);
  buf[7] = (VertAcc & 0xff);
  SendCANFrameToSerial(3205, buf);

  buf[0] = ((FuelLvl >> 8) & 0xff);
  buf[1] = (FuelLvl & 0xff);
  buf[2] = ((BattV >> 8) & 0xff);
  buf[3] = (BattV & 0xff);
  buf[4] = 0;
  buf[5] = 0;
  buf[6] = 0;
  buf[7] = 0;
  SendCANFrameToSerial(3206, buf);
}

void SendCANFrameToSerial(unsigned long canFrameId, const byte* frameData)
{
  // the 4 byte identifier at the beginning of each CAN frame
  // this is required for RealDash to 'catch-up' on ongoing stream of CAN frames
  const unsigned long serialBlockTag = 0x11223344;
  Serial3.write((const byte*)&serialBlockTag, 4);
//  SerialUSB.write((const byte*)&serialBlockTag, 4);

  // the CAN frame id number
  Serial3.write((const byte*)&canFrameId, 4);
//  SerialUSB.write((const byte*)&canFrameId, 4);

  // CAN frame payload
  Serial3.write(frameData, 8);
//  SerialUSB.write(frameData, 8);
}

//void printFrame(CAN_FRAME &frame) {
//   Serial3.print("ID: 0x");
//   Serial3.print(frame.id, HEX);
//   Serial3.print(" Len: ");
//   Serial3.print(frame.length);
//   Serial3.print(" Data: 0x");
//   for (int count = 0; count < frame.length; count++) {
//       Serial3.print(frame.data.bytes[count], HEX);
//       Serial3.print(" ");
//   }
//   Serial3.print("\r\n");
//}

#2

I suspect you’re running into the limitations of your serial speed. 9600 is a very slow rate. That’s in bits per second and there is a start and stop bit surrounding each 8 bit byte so you can divide that by 10. That’s 960 characters/bytes per second. Your CAN frames are being sent as 16 bytes each time. You send 7 of them each time. 16 * 7 is 112 bytes each time you send out an update. 960 / 112 is 8.5 updates per second possible. And that doesn’t take into account how you’ll be waiting around for long stretches for the serial buffer to clear. Then when you get back from that you can be assured you will have more frames in the CAN buffer since it took so long to send on serial. Things will back up because of this. I think what you need is a faster serial rate. 115200 is a popular choice.


#3

I would agree with @CollinK 9600 baud is no where near as fast as you need. When you consider the standard CANBUS is generally considerably faster than 9600 baud. If you are not using 115200 you will drop a significant portion of the packets or in your case back them up. Even the older pre-canbus protocols will back up at 9600 baud.


#4

Whelp, I feel dumb. That’s exactly what it was. Should’ve figured…

The delay is mostly gone now, but it still could be a bit more responsive. I’m not sure if it’s on the M2 side or the RealDash side.

Thanks for the help guys!


#5

@arbartz95, use a timer to send off the serial updates at a slower rate.

For example, limit it to 25updates per second? Or higher (Whatever works smoothly).

During the times where the serial is not being updated, let the M2 read the CANbus and update the variables for each parameter. You can then send all the desired sensors in one hit for each update which should display on screen smoothly and up to date!


#6

I should’ve posted an update. That’s exactly what I ended up doing now and it’s smooth as butter! Visually I can’t tell any delay/different in the tachometer on my phone vs the tach on the dash of the car. So that’s about as good as it gets.