Simhub Mazda 3 Instrument Cluster Support + ECU Simulator Support

SHMazda.h

#include <mcp_can.h>
#include <mcp_can_dfs.h>
#include <SPI.h>

#define MSCAN

#define DEBUG

// ---- LED Pin for OBD Simulator ---- //
const int HSCAN_LED = 41;
int hsCanLEDState = LOW;

// ---- CanBus Shield Config ---- //

const int SPI_CS_PIN1 = 53;

// -- Set CS pin -- //
MCP_CAN CAN1(SPI_CS_PIN1);


#ifdef MSCAN
  const int SPI_CS_PIN2 = 49;
  // -- Set CS pin -- //
  MCP_CAN CAN2(SPI_CS_PIN2);
#endif 


// ---- CanBus Shield Config ---- //


byte canDataFrame[8];
byte tempDataFrame[8];
byte msCanFrame[8];
byte msCanFrame1[8];
byte indicatorFrame[8]; // Driver Seat Belt Light



 // Sending this stops abs light being on.
byte absLight[8] = {0x80, 0x00, 0x20, 0x80, 0x00, 0x00, 0x00};

/* 
The below class currently initializes both the can shield,
and sends the actual data. This possibly should be split 
out and generalized later...

Some code inspiration from:
https://www.xsimulator.net/comm
y/threads/build-a-real-car-
instrument-cluster-for-racing-simulation-on-a-pc.11244/
*/

unsigned char flagRecv = 0;


//---------------------------------//

// -- Declare RPM, Speed -- //
int Speed;
int RPM;
int randRPM; // Needed for Ignition detection on some variants of OBD2 Diagnostic Tools

//---------------------------------//

// -- Enable OBD Simulator Support  -- //
#define OBDSIM

// -- Set PIDs --//
#define CAN_ID_PID          0x7DF
#define REPLY_ID            0x7E8
#define PAD 0x00

#define PID_ENGINE_RPM      0x0C
#define PID_VEHICLE_SPEED   0x0D
#define PID_COOLANT_TEMP    0x05


// Vin

// unsigned char vin[18] = "5YJXCBE42GFS00614"; // 2016 Tesla Model X

//---------------------------------//

// Power Relays for Voltage 12-14v, and Dash Relay
#define ignitionPin 44 // pin that controls Relay 1
#define dashPin 45 // pin that controls Relay 2

class mazda
{
private:
    int inputRPM;
    int rpm;
    int inputSpeed;
    int speed;
    int inputTemp;

    bool ignitionState;

    unsigned long speedTimer;
    unsigned long rpmTimer;
    unsigned long tempTimer;
    unsigned long ignTimer;
    unsigned long absTimer;
    unsigned long indicatorTimer;
    unsigned long hsCanLEDTimer;

    unsigned long odoTimer;
    unsigned long sendOdoTimer;

    // const byte interruptPin = 0;

public:

    float odoRaw;
    int iOdoRaw;
    long odoKm;

    bool flagRecv;

    void initializeRelay(){
        /*
        Relays need to be HIGH in setup, otherwise they'll trigger for a split second
        each time the Arduino is rebooted or SimHub first connects to it. 
        Note: When Ignition == on, relays need to be LOW to actually be "on"/. 
        */
        digitalWrite(ignitionPin, HIGH); 
        digitalWrite(dashPin, HIGH);
        pinMode(ignitionPin,OUTPUT);// define control pin as output
        pinMode(dashPin,OUTPUT);// define control pin as output

    }

    void initializeCan(){
        //  Note: Check the Clock on the CanBus Shield. Most are 16MHZ. 
        if (CAN1.begin(CAN_500KBPS, MCP_8MHz) == CAN_OK) FlowSerialDebugPrintLn("Can 1 init ok!!\r\n");
        else FlowSerialDebugPrintLn("Can 1 init fail!!\r\n");

        // Filter out incomming frames from instrument cluster. 
        
        /*
        * set mask, set both the mask to 0x3ff
        */
        CAN1.init_Mask(0, 0, 0x3ff);                         // there are 2 mask in mcp2515, you need to set both of them
        CAN1.init_Mask(1, 0, 0x3ff);


        /*
        * set filter, we can receive id from 0x04 ~ 0x09
        */
        CAN1.init_Filt(0, 0, 0x7DF);                          // OBD2 Scan tool request ID
        CAN1.init_Filt(1, 0, 0x7E8);                          // Main ECU/ECM Can ID

        CAN1.init_Filt(2, 0, 0x7E0);                          // ISOTP Flow control to VIN req. 
    //    CAN1.init_Filt(3, 0, 0x07);                          // there are 6 filter in mcp2515
    //    CAN1.init_Filt(4, 0, 0x08);                          // there are 6 filter in mcp2515
    //    CAN1.init_Filt(5, 0, 0x09);                          // there are 6 filter in mcp2515

      // Initialise LED Pin
      pinMode(HSCAN_LED, OUTPUT);


        #ifdef MSCAN
          if (CAN2.begin(CAN_125KBPS, MCP_8MHz) == CAN_OK) 
            FlowSerialDebugPrintLn("Can 2 init ok!!\r\n");
          else 
            FlowSerialDebugPrintLn("Can 2 init fail!!\r\n");
        #endif

    }

    void mscanData()
    {
      #ifdef MSCAN
      byte msCanFrame[8] = {0x00, 0x04, 0x50, 0x00, 0x00, 0x00, 0x00}; // Hand Brake Light
      CAN2.sendMsgBuf(0x433, 0, 8, msCanFrame);


      byte msCanFrame1[8] = {0x03, 0xa1, 0x00, 0x00, 0x00, 0x00, 0x00}; // Driver Seat Belt Light
      CAN2.sendMsgBuf(0x460, 0, 8, msCanFrame1);
      #endif
    }

    // TODO: Fix up this class to work properly with SimHub
    
    void indicator(int value, bool mode)
    {

		#ifdef MSCAN
      byte indicatorFrame[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // Indicator Frame

    if (mode == 1){
      /* Left Indicator*/
          if (value == 1) {
              if (millis() > indicatorTimer + 900) {
                  // Serial.print("Current indicator Timer: ");
                  // Serial.println(indicatorTimer);
                  indicatorTimer = millis();
                  indicatorFrame[0] = 0x20;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
              else if (millis() > indicatorTimer + 450) {
                  indicatorFrame[0] = 0x00;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
          }          

          /* Right Indicator*/
          else if (value == 2) {
              if (millis() > indicatorTimer + 900) {
                  // Serial.print("Current indicator Timer: ");
                  // Serial.println(indicatorTimer);
                  indicatorTimer = millis();
                  indicatorFrame[0] = 0x40;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
              else if (millis() > indicatorTimer + 450) {
                  indicatorFrame[0] = 0x00;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
                }
          }
       /* No Indicator*/
      else {
          if (millis() > indicatorTimer + 100) {
              indicatorTimer = millis();
              indicatorFrame[0] = 0x00;
              CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
          }
      }   
    /*
    Indicator Setup for SimHub Mode. Why is this different? 
    Because SimHub only sends the indicator value when it is *actually* blinking on.
    When it's not blinking, it sends a value of 0. 
    */
    if (mode == 0){
      /* Left Indicator*/
          if (value == 1) {
                  indicatorFrame[0] = 0x20;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
          else if (value == 2) {
                  indicatorFrame[0] = 0x40;
                  CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
              }
      else {
              indicatorFrame[0] = 0x00;
              CAN2.sendMsgBuf(0x265, 0, 8, indicatorFrame); // send Message
          }
      }
    }
    #endif

    void Send_Speed(int inputSpeed)
    {
        /* NOTE: This is an equation to ensure for example 50kmph actually shows as
           50kmph on the Instrument Cluster. You may need to fine-tune 1.029 with
           your own cluster to get values correct. */

        int speed = inputSpeed /  1.029;   
        // -- Set values -- //
        canDataFrame[4] = (speed * 100 ) / 256; // inputSpeed
        canDataFrame[5] = (speed + 100) % 256; // inputSpeed
        canDataFrame[6] = 0x00;
        canDataFrame[7] = 0x00;

        actuallySendData();

    }

    void Send_RPM(int inputRPM)
    {
        // -- Set values -- //

        /* NOTE: This is an equation to ensure for example 5000RPM actuall shows as
           5000RPM on the Instrument Cluster. You may need to fine-tune 4.167 with
           your own cluster to get values correct. */
           
        int rpm = inputRPM / 4.167;
        canDataFrame[0] = (rpm * 4) / 256; // rpm
        canDataFrame[1] = (rpm * 4) % 256; // rpm

        // FlowSerialDebugPrintLn("INPUT RPM         : " + String(inputRPM));

        actuallySendData();

                // I think I can get rid of the below code, and instead cover it in
                // the Send_Temp function below.....
                //   // -- Engine and battery lights will turn off when engine has started -- //
                // if (inputRPM > 200){
                //     tempDataFrame[3] = 0xFF; // check engine light + coolant, oil and battery OFF
                    
                //     CAN1.sendMsgBuf(0x420, 0, 8, tempDataFrame); // send Message
                //     // FlowSerialDebugPrintLn("battery light OFF. RPM OVER 100");
                // }
    }

    void Odo_Calc(int inputSpeed)
    {
      while(millis() > odoTimer + 1000){
        Serial.print("ignition State: "); // not working yet. 
        Serial.println(ignitionState);
        odoTimer = millis();

        Serial.print("Speed: ");
        Serial.println(inputSpeed);

        odoRaw += ((float)inputSpeed) / 3600.0;

        Serial.print("odoRaw: ");
        Serial.println(odoRaw);

        odoKm = odoRaw + 136249; // For a more realisitic ODO starting point. 
        Serial.print("odoKM: ");
        Serial.println(odoKm);

        if(millis() > sendOdoTimer + 3000){
          sendOdoTimer = millis();
          // Serial.println("got here");

          // If we take a CANbus example of "4F2#02029800" and apply ODO formula for it (98 hex is 152 dec), we get "((02*256+02)*256+152)*1000" = 131,736,000 M Odometer
          // If we take a CANbus example of "4F2#02029800" and apply the ODO formula for it (98 hex is 152 dec), we get "((a*256+b)*256+c)*1000" = 131,736,000 M Odometer

          canDataFrame[0] = (odoKm >> 16) & 255; // ODO 1st byte with bitshifting
          canDataFrame[1] = (odoKm >> 8) & 255;  // ODO 2nd byte with bitshifting
          canDataFrame[2] = (odoKm >> 0 ) & 255; // ODO 3rd byte with bitshifting
          canDataFrame[3] = 0x00;                // ODO on real vehicle is only 4 bytes long

          CAN1.sendMsgBuf(0x4F2, 0, 4, canDataFrame); // 

          // Simply for printing the frames to serial buffer for debugging
          for(int i = 0; i < sizeof(canDataFrame); i++)
          {
              Serial.print("0x"); 
              Serial.print(canDataFrame[i], HEX);
              Serial.print(" ");
          }
          Serial.println("");
        } 
      }
    }

    void actuallySendData(){
        while(millis() > speedTimer + 20){ // Send message every 20 msec
            speedTimer = millis();
            CAN1.sendMsgBuf(0x201, 0, 8, canDataFrame); // Send Message.
        }
        while(millis() > absTimer + 25){   // Send message every 25 msec
            absTimer = millis();
            CAN1.sendMsgBuf(0x212, 0, 8, absLight); // Disable ABS Light.
        }
    }

    void Send_Temp(int inputTemp, int inputRPM)
    {

    // tempDataFrame[0] = 0x98; // Temp Gauge Data in Celsius. 152 celsius. 

    tempDataFrame[0] = inputTemp; // Temp Gauge Data in Celsius
    tempDataFrame[1] = 0x00;
    
    // FlowSerialDebugPrintLn(" " + String(tempDataFrame));

        if (inputRPM > 0) { // We don't want to send temp if vehicle is "off".
            while (millis() > tempTimer + 100){ // Send message every 100 msec
                tempDataFrame[3] = 0xFF; // check engine light + coolant, oil and battery OFF

                tempTimer = millis();
                CAN1.sendMsgBuf(0x420, 0, 8, tempDataFrame); // send Message
            }
        }
    }


    void Ignition_State(bool ignitionState)
    {
      if (ignitionState == true)
      {
        digitalWrite(ignitionPin,LOW);
        digitalWrite(dashPin,LOW);
      }
      else
      /* We want to avoid turning off the Instrument Cluster if gauges haven't got back to 0 yet.
         So we wait half a second for this.     
         This may not be needed if I decide to not put the cluster on a relay...*/
        while (millis() > ignTimer + 500) {
                ignTimer = millis();
                digitalWrite(ignitionPin,HIGH);
                digitalWrite(dashPin,HIGH);
            }
    }


    // TODO: GEAR information. 
    // My cluster does not have Gear info so haven't done yet



void taskCanRecv(int inputSpeed, int inputRPM, bool ignitionState)
{
    unsigned char len = 0;
    unsigned char buf[8];

    // if(flagRecv)
    if(CAN_MSGAVAIL == CAN1.checkReceive())                   // check if get data
    {
        CAN1.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf
        unsigned long canId = CAN1.getCanId();
        unsigned long buffer = *buf;


        #ifdef DEBUG
            // FlowSerialDebugPrintLn("Frame Received: ");
            Serial.print("Frame Received: ");

            // FlowSerialDebugPrintLn(String(canId));
            // FlowSerialDebugPrintLn("    ");

            // for(int i = 0; i<len; i++)    // print the data
            // {
            //     FlowSerialDebugPrintLn("0x");
            //     FlowSerialDebugPrintLn(buf[i], HEX);
            //     FlowSerialDebugPrintLn(" ");
            // }
            // FlowSerialDebugPrintLnln();
        #endif

        #ifdef OBDSIM
            if(canId == 0x7DF || canId == 0x7E0)
            {
                // This needs tidying up and converted to FlowSerialDebugPrint instead..
                Serial.print(canId, HEX);
                Serial.print("    ");
                for(int i = 0; i < sizeof(buf); i++)
                {
                    Serial.print("0x"); 
                    Serial.print(buf[i], HEX);
                    Serial.print(" ");
                } 
                Serial.println("");              
                obdReq(buf, inputRPM, inputSpeed, ignitionState);
            }
        #endif

    }
}


// -- OBD Simulator Support  -- //
// Originally from https://github.com/coryjfowler/MCP_CAN_lib/blob/master/examples/OBD_Sim/OBD_Sim.ino

void obdReq(byte *data, int inputRPM, int inputSpeed, bool ignitionState){
  byte numofBytes = data[0];
  byte mode = data[1] & 0x0F;
  byte pid = data[2];
  bool tx = false;
  int rpmResponse;

  int RPM = inputRPM; // Only need RPM. We use "inputSpeed" for Speed.

  byte txData[] = {0x00,(0x40 | mode),pid,PAD,PAD,PAD,PAD,PAD};
  //=============================================================================
  // MODE $01 - Show current data
  //=============================================================================
  if(mode == 0x01){
    if(pid == 0x00){        // Supported PIDs 01-20
      txData[0] = 0x06;
      
      txData[3] = 0x80;
      txData[4] = 0x38;
      txData[5] = 0x00;
      txData[6] = 0x01;
      tx = true;
    }
    else if(pid == 0x01){    // Monitor status since DTCs cleared.
      bool MIL = false;
      /* 
      If MIL = true:
      0x6 0x41 0x1 0x85 0x7 0xFF 0x0 0x0
      If MIL = false:
      0x6 0x41 0x1 0x5 0x7 0xFF 0x0 0x0 
      */
      byte DTC = 5;
      txData[0] = 0x06;
      
      txData[3] = (MIL << 7) | (DTC & 0x7F);
      txData[4] = 0x07;
      txData[5] = 0xFF;
      txData[6] = 0x00;
      tx = true;
    }
    else if(pid == 0x0C){    // Engine RPM
      txData[0] = 0x04;

      if (RPM > 100) {
        randRPM = random((RPM - 50),(RPM + 50)); // generate random number to fluctuate ~ 100 RPM
        Serial.print("Random RPM Sent Value: ");
        Serial.println(randRPM);
        rpmResponse = randRPM;
      }
      else rpmResponse = RPM;

      txData[3] = rpmResponse / 256; // rpm
      txData[4] = rpmResponse % 256; // rpm

      tx = true;

      #ifdef DEBUG
          Serial.print("RPM Response: ");
          Serial.print(RPM);
          Serial.println(" ");
      #endif
    }
    else if(pid == 0x0D){    // Vehicle speed
      txData[0] = 0x04;

      /*  We want to ensure the last Speed value sent to OBD Device is 0 as 
          a car should not have the Ignition turned off while in motion.
          On second thought..Perhaps this logic should be elsewhere....  */

      // if (ignitionState == false) {
      //     txData[3] = 0;
      //     Speed = 0;    
      // }
      // else {
        txData[3] = inputSpeed;
        Speed = inputSpeed;
      // }
      

      // txData[3] = 0xFA;
      txData[4] = 0x00;
      tx = true;

      #ifdef DEBUG
          Serial.print("Speed Response: ");
          Serial.print(Speed);
          Serial.println(" ");
      #endif
    }
    else if(pid == 0x20){    // Supported PIDs 21-40
      txData[0] = 0x06;
      
      txData[3] = 0x80;
      txData[4] = 0x00;
      txData[5] = 0x00;
      txData[6] = 0x01;
      tx = true;
    }
    else{
      unsupported(mode, pid);
    }
  }


  //=============================================================================
  // MODE $09 - Request vehicle information
  //=============================================================================
  else if(mode == 0x09){

    if(pid == 0x00){        // Supported PIDs 01-20
      txData[0] = 0x06;
      
      txData[3] = 0x54;
      txData[4] = 0x40;
      txData[5] = 0x00;
      txData[6] = 0x00;
      tx = true;
    }

    // <---- Hacked up method to do a VIN Response. ----> //
    //        Probably should redo doing ISO-TP           //
    else if(pid == 0x02){    // VIN (17 to 20 Bytes). 
      byte vinResp1[] = {0x10, 0x14, 0x49, 0x02, 0x01,vin[0],vin[1],vin[2]};
      CAN1.sendMsgBuf(REPLY_ID,0,8,vinResp1);
      } 
    }  
  
  else if(CAN1.getCanId() == 0x7e0 && (numofBytes == 0x30)) // Flow control frame. 
  {
    Serial.println("VIN Resp.");
      byte vinResp2[] = {0x21,vin[3],vin[4],vin[5],vin[6],vin[7],vin[8],vin[9]};
      CAN1.sendMsgBuf(REPLY_ID,0,8,vinResp2);
      byte vinResp3[] = {0x22,vin[10],vin[11],vin[12],vin[13],vin[14],vin[15],vin[16]};
      CAN1.sendMsgBuf(REPLY_ID,0,8,vinResp3);
  }

  // ---------------------------------------------------// 

  else { 
    unsupported(mode, pid);
  }

// Serial.print("TX STATE:");
// Serial.println(tx);

/* The problem is that we only want to send data if:
   - Ignition is ON.
   We DON'T want to send data unless:
   - Ignition is False, HOWEVER
   it's probably ideal if last speed value is set to 0 before ignition off. 
   This is because some OBD2 Diagnostic Tools may still report the last received speed unless the
   vehicle was stationary before hand....
*/ 

  // Send the data 
  if(tx)
    CAN1.sendMsgBuf(REPLY_ID, 0, 8, txData);
    
    flagRecv = true;

    #ifdef DEBUG
        Serial.print("Frame Response: ");
        Serial.print(REPLY_ID, HEX);
        Serial.print("    ");

        for(int i = 0; i < sizeof(txData); i++)
        {
            Serial.print("0x"); 
            Serial.print(txData[i], HEX);
            Serial.print(" ");
        }
        Serial.println("");
        Serial.println("");
    #endif
    tx = false;
  }

// Generic debug serial output
void unsupported(byte mode, byte pid){
  negAck(mode, 0x12);
  unsupportedPrint(mode, pid);  
}

// Generic debug serial output
void negAck(byte mode, byte reason){
  byte txData[] = {0x03,0x7F,mode,reason,PAD,PAD,PAD,PAD};
  CAN1.sendMsgBuf(REPLY_ID, 0, 8, txData);
}


// Generic debug serial output
void unsupportedPrint(byte mode, byte pid){
  char msgstring[64];
  sprintf(msgstring, "Mode $%02X: Unsupported PID $%02X requested!", mode, pid);
  Serial.println(msgstring);
}
};