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