Queries over the CAN bus to see if anything responds to a VIN number request

M2VIN2SD.ino

//Macchina LLC
//Collin Kidder
//Queries over the CAN bus to see if anything responds to a VIN number request

#include <Arduino_Due_SD_HSMCI.h>
#include "Arduino.h"
#include <M2_12VIO.h>
#include <due_can.h>
#include <iso-tp.h>

FileStore FS;
M2_12VIO M2IO;

IsoTp isotp0(&Can0);

#define ECU_STARTID   0x7E0
#define ECU_ENDID     0x7E7
#define REPLY_OFFSET  8 //might be 0x10 or even 0x20 on your car

struct Message_t TxMsg, RxMsg;

bool flashOn = false;
bool inTest = false;
bool success = false;
uint32_t tick = 0;
char write_buffer[200];

char fileName[] = "VIN_00.txt";  // Base filename for logging.

void cycleLEDs()
{
  for (int i = 14; i < 19; i++) digitalWrite(i, HIGH);//turn all LEDs off
  flashOn = !flashOn;
  if (flashOn)
  {
    if (inTest) digitalWrite(16, LOW);
    else 
    {
      if (success) digitalWrite(18, LOW);
      else digitalWrite(14, LOW);
    }
  }
  else
  {
    if (inTest) digitalWrite(17, LOW);
  }
}

void setupCAN()
{
  Can0.enable();
  SerialUSB.println();
  uint32_t canSpeed = Can0.beginAutoSpeed(); //try to figure out the proper speed automatically.
  if (canSpeed > 0)
  {
    SerialUSB.print("Autobaud detected speed is ");
    SerialUSB.println(canSpeed);  
  }
  else
  {
    Can0.enable();
    Can0.begin(500000, 255);
    SerialUSB.println("Auto speed detect failed. Using 500k");
  }
  Can0.setListenOnlyMode(false); //important! autobaud enables listen only
  SerialUSB.println();

  for (int filter = 0; filter < 3; filter++) {
      Can0.setRXFilter(filter, 0, 0, true);
  }  
  //standard
  for (int filter = 3; filter < 7; filter++) {
      Can0.setRXFilter(filter, 0, 0, false);
  }  

  TxMsg.Buffer=(uint8_t *)calloc(MAX_MSGBUF,sizeof(uint8_t));
  RxMsg.Buffer=(uint8_t *)calloc(MAX_MSGBUF,sizeof(uint8_t));
}

void queryECU(uint32_t id, IsoTp *iso)
{
  String str=String("Querying ECU at 0x");
  str.concat(String(id, HEX));
  str.concat("\r\n");
  SerialUSB.print(str);
  FS.Write(str.c_str());
  FS.Flush();

  TxMsg.len = 2;
  TxMsg.tx_id = id;
  TxMsg.rx_id = id + REPLY_OFFSET;
  TxMsg.Buffer[0] = 9; //get vehicle info
  TxMsg.Buffer[1] = 2; //request VIN number  
  iso->send(&TxMsg);
  SerialUSB.print("Reported VIN#: ");
  FS.Write("Reported VIN#: ");
  RxMsg.tx_id = id;
  RxMsg.rx_id = id + REPLY_OFFSET;
  iso->receive(&RxMsg);
  
  if (RxMsg.tp_state == ISOTP_FINISHED)
  {
    for (int i = 2; i < RxMsg.len; i++)  //the first two bytes are the mode and PID so skip those here.
    {
      if (RxMsg.Buffer[i] != 0) 
      {
        SerialUSB.write(RxMsg.Buffer[i]);
        FS.Write(RxMsg.Buffer[i]);
        success = true;
      }
    }
    SerialUSB.println();
    FS.Write("\r\n");
    FS.Flush();
  }
  else
  {
    SerialUSB.println("ERR!");
    FS.Write("ERR!\r\n");
    FS.Flush();
  }
  RxMsg.tp_state = ISOTP_IDLE;
  
  SerialUSB.println();
}

void runTest()
{
  /*
  The OBDII standard says that ECUs should respond to ids in the range 0x7E0 to 0x7E7. They should
  respond 0x08 higher than their ID so 0x7E0 responds with ID 0x7E8. Some cars don't follow this standard
  very closely and thus use addresses outside this range and return offsets other than 0x8. 0x10 is common too.
  Modify REPLY_OFFSET to change the offset. Modify ECU_STARTID and ECU_ENDID to change the range.
  */
  FS.Open("0:", fileName, true);
  FS.GoToEnd();
  FS.Write("----==== ECUS ====----\n");

  SerialUSB.println("----==== ECUS ====----");
  inTest = true;
  uint32_t ecuID;
  for (ecuID = ECU_STARTID; ecuID <= ECU_ENDID; ecuID++)
  {
    cycleLEDs();
    queryECU(ecuID, &isotp0);
  }
  FS.Close();
  inTest = false;
}


void setup()
{
  pinMode(Button1, INPUT_PULLUP);//initialize button 1 for input
  
  for(int i=14; i<=18; i++)//initialize LEDs, 14 is red 18 is green
  {
    pinMode(i, OUTPUT);
    digitalWrite(i, HIGH);
  }

  delay(2000);
  digitalWrite(14, LOW);//turn red LED on
  flashOn = true;
  success = false;
  SerialUSB.println("Standalone VIN Reader for M2");
  SerialUSB.println("****************************");
  SerialUSB.println();

  SD.Init();
  FS.Init();

  // Construct the filename to be the next incrementally indexed filename in the set [00-99].
  for (byte i = 1; i <= 99; i++)
  {
    // check before modifying target filename.
    if (SD.FileExists(fileName))
    {
      // the filename exists so increment the 2 digit filename index.
      fileName[4] = i/10 + '0';
      fileName[5] = i%10 + '0';
    } else {
      break;  // the filename doesn't exist so break out of the for loop.
    }
  }

  FS.CreateNew("0:",fileName);//create file to use for output

  setupCAN();

  runTest();
}

void loop()
{
  if (digitalRead(Button1)==LOW)
  {
     SerialUSB.println("Rerunning the test!");
     for (int i = 14; i < 19; i++) digitalWrite(i, HIGH);//turn all LEDs off
     digitalWrite(14, LOW); //turn on first RED light
     flashOn = true;
     success = false;
     runTest();
  }
  if (tick + 500 < millis())
  {
    tick = millis();
    cycleLEDs();
  }
}