TvrECUOBD.ino

// CAN Receive Example
//
#include <mcp_can.h>
#include <SPI.h>
#include <SoftwareSerial.h>

//OBD Throttle position array in progmem, use MBE response as index
const byte t_TPOBD[256] PROGMEM = {0xA,0xA,0xA,0xB,0xB,0xB,0xC,0xC,0xC,0xD,0xD,0xD,0xE,0xE,0xE,0xF,0xF,0x10,0x10,0x10,0x11,0x11,0x11,0x12,0x12,0x12,0x13,0x13,0x13,0x14,0x14,0x15,0x15,0x15
,0x16,0x16,0x16,0x17,0x17,0x17,0x18,0x18,0x18,0x19,0x19,0x1A,0x1A,0x1A,0x1B,0x1B,0x1B,0x1C,0x1C,0x1C,0x1D,0x1D,0x1D,0x1E,0x1E,0x1E,0x1F,0x1F,0x20,0x20
,0x20,0x21,0x21,0x21,0x22,0x22,0x22,0x23,0x23,0x23,0x24,0x24,0x25,0x25,0x25,0x26,0x26,0x26,0x27,0x27,0x27,0x28,0x28,0x28,0x29,0x29,0x29,0x2A,0x2A,0x2B
,0x2B,0x2B,0x2C,0x2C,0x2C,0x2D,0x2D,0x2D,0x2E,0x2E,0x2E,0x2F,0x2F,0x30,0x30,0x30,0x31,0x31,0x31,0x32,0x32,0x32,0x33,0x33,0x33,0x34,0x34,0x34,0x35,0x35
,0x36,0x36,0x36,0x37,0x37,0x37,0x38,0x38,0x38,0x39,0x39,0x39,0x3A,0x3A,0x3B,0x3B,0x3B,0x3C,0x3C,0x3C,0x3D,0x3D,0x3D,0x3E,0x3E,0x3E,0x3F,0x3F,0x3F,0x40
,0x40,0x41,0x41,0x41,0x42,0x42,0x42,0x43,0x43,0x43,0x44,0x44,0x44,0x45,0x45,0x46,0x46,0x46,0x47,0x47,0x47,0x48,0x48,0x48,0x49,0x49,0x49,0x4A,0x4A,0x4A
,0x4B,0x4B,0x4C,0x4C,0x4C,0x4D,0x4D,0x4D,0x4E,0x4E,0x4E,0x4F,0x4F,0x4F,0x50,0x50,0x51,0x51,0x51,0x52,0x52,0x52,0x53,0x53,0x53,0x54,0x54,0x54,0x55,0x55
,0x55,0x56,0x56,0x57,0x57,0x57,0x58,0x58,0x58,0x59,0x59,0x59,0x5A,0x5A,0x5A,0x5B,0x5B,0x5C,0x5C,0x5C,0x5D,0x5D,0x5D,0x5E,0x5E,0x5E,0x5F,0x5F,0x5F,0x60
,0x60,0x61,0x61,0x61,0x62,0x62,0x62,0x63,0x63,0x63,0x64,0x64};

//RPM array in Progmem, use to calculate RPM value from MBE response as indices
const byte t_mbeRPMLow[256] PROGMEM = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52
,53,54,55,56,57,58,59,60,61,62,63,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101
,102,103,104,105,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138
,139,140,141,142,143,144,145,146,147,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175
,176,177,178,179,180,181,182,183,184,185,186,187,188,189,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212
,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227,228,229,230,230,231,232,233,234,235,236,237,238,239,240,241,242,243,244,245,246,247,248,249};

// BARO OBD array in Progmem, use to calculate baro value straight from MBE TO OBD
const byte t_mbeBARO_OBD[256] PROGMEM = {0x0,0x0,0x1,0x1,0x2,0x2,0x3,0x3,0x3,0x4,0x4,0x5,0x5,0x5,0x6,0x6,0x7,0x7,0x8,0x8,0x8,0x9,0x9,0xA,0xA,0xA,0xB,0xB,0xC,0xC,0xD,0xD,0xD,0xE,0xE,0xF,0xF
,0xF,0x10,0x10,0x11,0x11,0x12,0x12,0x12,0x13,0x13,0x14,0x14,0x14,0x15,0x15,0x16,0x16,0x17,0x17,0x17,0x18,0x18,0x19,0x19,0x19,0x1A,0x1A,0x1B,0x1B,0x1C
,0x1C,0x1C,0x1D,0x1D,0x1E,0x1E,0x1E,0x1F,0x1F,0x20,0x20,0x21,0x21,0x21,0x22,0x22,0x23,0x23,0x23,0x24,0x24,0x25,0x25,0x26,0x26,0x26,0x27,0x27,0x28,0x28
,0x28,0x29,0x29,0x2A,0x2A,0x2B,0x2B,0x2B,0x2C,0x2C,0x2D,0x2D,0x2D,0x2E,0x2E,0x2F,0x2F,0x30,0x30,0x30,0x31,0x31,0x32,0x32,0x32,0x33,0x33,0x34,0x34,0x35
,0x35,0x35,0x36,0x36,0x37,0x37,0x37,0x38,0x38,0x39,0x39,0x3A,0x3A,0x3A,0x3B,0x3B,0x3C,0x3C,0x3C,0x3D,0x3D,0x3E,0x3E,0x3F,0x3F,0x3F,0x40,0x40,0x41,0x41
,0x41,0x42,0x42,0x43,0x43,0x44,0x44,0x44,0x45,0x45,0x46,0x46,0x46,0x47,0x47,0x48,0x48,0x49,0x49,0x49,0x4A,0x4A,0x4B,0x4B,0x4B,0x4C,0x4C,0x4D,0x4D,0x4E
,0x4E,0x4E,0x4F,0x4F,0x50,0x50,0x50,0x51,0x51,0x52,0x52,0x53,0x53,0x53,0x54,0x54,0x55,0x55,0x55,0x56,0x56,0x57,0x57,0x58,0x58,0x58,0x59,0x59,0x5A,0x5A
,0x5A,0x5B,0x5B,0x5C,0x5C,0x5D,0x5D,0x5D,0x5E,0x5E,0x5F,0x5F,0x5F,0x60,0x60,0x61,0x61,0x62,0x62,0x62,0x63,0x63,0x64,0x64,0x64,0x65,0x65,0x66,0x66,0x67
,0x67,0x67,0x68,0x68,0x69,0x69,0x69,0x6A,0x6A};

// IGNITIION OBD array in Progmem, use to calculate ignition value straight from MBE to OBD
const byte t_TPIgnition_OBD[256] PROGMEM = {0x8,0x8,0x9,0x9,0xA,0xA,0xB,0xB,0xC,0xC,0xD,0xD,0xE,0xE,0xF,0xF,0x10,0x10,0x10,0x11,0x11,0x12,0x12,0x13,0x13,0x14,0x14,0x15,0x15,0x16,0x16,0x17,0x17
,0x18,0x18,0x18,0x19,0x19,0x1A,0x1A,0x1B,0x1B,0x1C,0x1C,0x1D,0x1D,0x1E,0x1E,0x1F,0x1F,0x20,0x20,0x20,0x21,0x21,0x22,0x22,0x23,0x23,0x24,0x24,0x25,0x25
,0x26,0x26,0x27,0x27,0x28,0x28,0x28,0x29,0x29,0x2A,0x2A,0x2B,0x2B,0x2C,0x2C,0x2D,0x2D,0x2E,0x2E,0x2F,0x2F,0x30,0x30,0x30,0x31,0x31,0x32,0x32,0x33,0x33
,0x34,0x34,0x35,0x35,0x36,0x36,0x37,0x37,0x38,0x38,0x38,0x39,0x39,0x3A,0x3A,0x3B,0x3B,0x3C,0x3C,0x3D,0x3D,0x3E,0x3E,0x3F,0x3F,0x40,0x40,0x40,0x41,0x41
,0x42,0x42,0x43,0x43,0x44,0x44,0x45,0x45,0x46,0x46,0x47,0x47,0x48,0x48,0x48,0x49,0x49,0x4A,0x4A,0x4B,0x4B,0x4C,0x4C,0x4D,0x4D,0x4E,0x4E,0x4F,0x4F,0x50
,0x50,0x50,0x51,0x51,0x52,0x52,0x53,0x53,0x54,0x54,0x55,0x55,0x56,0x56,0x57,0x57,0x58,0x58,0x58,0x59,0x59,0x5A,0x5A,0x5B,0x5B,0x5C,0x5C,0x5D,0x5D,0x5E
,0x5E,0x5F,0x5F,0x60,0x60,0x60,0x61,0x61,0x62,0x62,0x63,0x63,0x64,0x64,0x65,0x65,0x66,0x66,0x67,0x67,0x68,0x68,0x68,0x69,0x69,0x6A,0x6A,0x6B,0x6B,0x6C
,0x6C,0x6D,0x6D,0x6E,0x6E,0x6F,0x6F,0x70,0x70,0x70,0x71,0x71,0x72,0x72,0x73,0x73,0x74,0x74,0x75,0x75,0x76,0x76,0x77,0x77,0x78,0x78,0x78,0x79,0x79,0x7A
,0x7A,0x7B,0x7B,0x7C,0x7C,0x7D,0x7D,0x7E,0x7E,0x7F,0x7F,0x80,0x80};

// AIR and WATER TEMP array in Progmem, use to calculate air/water temp straight from MBE to OBD
const byte t_mbeAIRWATER_OBD[256] PROGMEM = {0xA,0xA,0xB,0xB,0xC,0xD,0xD,0xE,0xF,0xF,0x10,0x10,0x11,0x12,0x12,0x13,0x14,0x14,0x15,0x15,0x16,0x17,0x17,0x18,0x19,0x19,0x1A,0x1A,0x1B,0x1C,0x1C
,0x1D,0x1E,0x1E,0x1F,0x20,0x20,0x21,0x21,0x22,0x23,0x23,0x24,0x25,0x25,0x26,0x26,0x27,0x28,0x28,0x29,0x2A,0x2A,0x2B,0x2B,0x2C,0x2D,0x2D,0x2E,0x2F
,0x2F,0x30,0x30,0x31,0x32,0x32,0x33,0x34,0x34,0x35,0x35,0x36,0x37,0x37,0x38,0x39,0x39,0x3A,0x3A,0x3B,0x3C,0x3C,0x3D,0x3E,0x3E,0x3F,0x40,0x40,0x41
,0x41,0x42,0x43,0x43,0x44,0x45,0x45,0x46,0x46,0x47,0x48,0x48,0x49,0x4A,0x4A,0x4B,0x4B,0x4C,0x4D,0x4D,0x4E,0x4F,0x4F,0x50,0x50,0x51,0x52,0x52,0x53
,0x54,0x54,0x55,0x55,0x56,0x57,0x57,0x58,0x59,0x59,0x5A,0x5A,0x5B,0x5C,0x5C,0x5D,0x5E,0x5E,0x5F,0x60,0x60,0x61,0x61,0x62,0x63,0x63,0x64,0x65,0x65
,0x66,0x66,0x67,0x68,0x68,0x69,0x6A,0x6A,0x6B,0x6B,0x6C,0x6D,0x6D,0x6E,0x6F,0x6F,0x70,0x70,0x71,0x72,0x72,0x73,0x74,0x74,0x75,0x75,0x76,0x77,0x77
,0x78,0x79,0x79,0x7A,0x7A,0x7B,0x7C,0x7C,0x7D,0x7E,0x7E,0x7F,0x80,0x80,0x81,0x81,0x82,0x83,0x83,0x84,0x85,0x85,0x86,0x86,0x87,0x88,0x88,0x89,0x8A
,0x8A,0x8B,0x8B,0x8C,0x8D,0x8D,0x8E,0x8F,0x8F,0x90,0x90,0x91,0x92,0x92,0x93,0x94,0x94,0x95,0x95,0x96,0x97,0x97,0x98,0x99,0x99,0x9A,0x9A,0x9B,0x9C
,0x9C,0x9D,0x9E,0x9E,0x9F,0xA0,0xA0,0xA1,0xA1,0xA2,0xA3,0xA3,0xA4,0xA5,0xA5,0xA6,0xA6,0xA7,0xA8,0xA8,0xA9,0xAA};

// the cs pin of the version after v1.1 is default to D9
// v0.9b and v1.0 is default D10
const int SPI_CS_PIN = 9;
MCP_CAN CAN(SPI_CS_PIN); // Set CS pin
// DIY ShieldMCP_CAN CAN0(10);                               // Set CS to pin 10

// MBE Response data
byte bMBE_RPMLow = 0;
byte bMBE_RPMHigh = 0;
byte bMBE_TP1 = 0;
byte bMBE_TP2 = 0;
byte bMBE_BATT = 0;
byte bMBE_LAM1 = 0;
byte bMBE_LAM2 = 0;
byte bMBE_BARO = 0;
byte bMBE_IGN = 0;
byte bMBE_AIR = 0;
byte bMBE_WATER = 0;
byte bMBE_ADAPT1 = 0;
byte bMBE_ADAPT2 = 0;
byte bMBE_INJ1Low = 0;
byte bMBE_INJ1High = 0;
byte bMBE_INJ2Low = 0;
byte bMBE_INJ2High = 0;

// Temporary ECU response values
byte bMBEResponse1;
byte bMBEResponse2;

//OBD Calculated Values
byte bOBD_ValueA;
byte bOBD_ValueB;
byte bOBD_ValueC;
byte bOBD_ValueD;

SoftwareSerial gtSerial(6, 7); // Arduino RX, Arduino TX

void debPrint (String message) {
  gtSerial.print (message);
}

void debPrintb (byte message) {
  gtSerial.print (message,HEX);
}

void debWrite (byte message) {
  gtSerial.write (message);
}

void debPrintln (String message) {
  gtSerial.println (message);
}

void setup()
{
  gtSerial.begin(4800);  // software serial port
  Serial.begin(4800,SERIAL_8N1);
  Serial.setTimeout(1000);
  gtSerial.setTimeout(1000);
  
  while (CAN_OK != CAN.begin(CAN_500KBPS))              // init can bus : baudrate = 500k
  {
    debPrintln("CAN BUS Shield init fail");
    debPrintln(" Init CAN BUS Shield again");
    delay(100);
  }
  debPrintln("CAN BUS Shield init ok!");
  // wait 1 second to allow everything to settle
  delay(1000);

  pinMode(0, INPUT_PULLUP); 
}

void send (byte howmany, byte val1, byte val2, byte val3, byte val4, byte val5, byte val6, byte val7) {
  byte data[8];
  data[0] = howmany;
  data[1] = val1;
  data[2] = val2;
  data[3] = val3;
  data[4] = val4;
  data[5] = val5;
  data[6] = val6;
  data[7] = val7;
  
  CAN.sendMsgBuf(0x7E8, 0, howmany+1, data);    
}

void   ClearSerialIN() {
  // repeat getting data from input buffer until empty
  while(Serial.available() > 0) {
    char t = Serial.read();
  }
}

byte MBERequestOne (byte bRequest1, byte bOriginalValue) {
  byte iCount = 0;
  unsigned long timeout;
  byte bReturn;

  // set return value to original just in case we don't get anything back from ECU in time
  bReturn = bOriginalValue;
  
  //debPrintln("Starting MBE Request ONE");
  // Clear any data received since last time
  ClearSerialIN();

  //debPrintln("Sending MBE request ONE");
  // make serial request 
  Serial.write(bRequest1);
  //debPrintln("Flushing serial port");
  Serial.flush();
  
  //debPrintln("Waiting for data from ECU");
  
  timeout = millis() + 1000; // Allow ECU 1s to respond (TWEAK LATER)
  while ((iCount < 1) && (millis() < timeout)) {
    //debPrintln("Loop 1");
    while ((Serial.available() != 0) && (millis() < timeout)) {
      iCount = iCount + 1;
      //debPrint("Loop 2:");
      //debPrintb(iCount);
      //debPrintln("");
      if (iCount == 1) {
        //debPrintln("Got 1 bit of data");
        //debPrintln("Getting data from ECU");
        bReturn = Serial.read();
        //debPrint("Read: ");
        //debWrite(bReturn);
        //debPrintb(bReturn);
        break; 
      }
    }
  }
  if (iCount == 0) {
    debPrintln(" !!!! MBE ONE NO DATA !!!!");
  }
  return bReturn;
}

boolean MBERequestTwo (byte bRequest1, byte bRequest2) {
  byte iCount = 0;
  unsigned long timeout;
  boolean bResponse = false;
  byte bRequest[] = {0,0};
  int iSent;
  
  //debPrintln("Starting MBE Request TWO");
  // Clear any data received since last time
  ClearSerialIN();

  //debPrintln("Sending MBE request TWO");
  // make serial request
  bRequest[0] = bRequest1;
  bRequest[1] = bRequest2;
  iSent = Serial.write(bRequest,sizeof(bRequest));
  //debPrint("Sent ");
  //debPrintb(iSent);
  //debPrintln(" bytes");
  //debPrintln("Flushing serial port");
  Serial.flush();
  delay(100); // allow ECU time to respond - without this we don't appear to get responses in time
  //debPrintln("Waiting for data from ECU");
  
  timeout = millis() + 1000; // Allow ECU 1s to respond (TWEAK LATER)
  while ((iCount < 1) && (millis() < timeout)) {
    while ((Serial.available() != 0) && (millis() < timeout)) {
      iCount = iCount + 1;
      //debPrintln("");
      //debPrint("iCount=");
      //debPrintb(iCount);
      //debPrintln("");
      if (iCount == 1) {
        //debPrintln("Got 1 bit of data");
        //debPrintln("Getting data from ECU");
        bMBEResponse1 = Serial.read();
        //debPrint("Read: ");
        //debWrite(bMBEResponse1);
        //debPrintb(bMBEResponse1);
      } else if (iCount == 2) {
        //debPrintln("Got 2 bits of data");
        //debPrintln("Getting data from ECU");
        bMBEResponse2 = Serial.read();
        //debPrint("Read: ");
        //debWrite(bMBEResponse2);
        //debPrintb(bMBEResponse2);
        bResponse = true;
        break; 
      }
    }
  }
  if (iCount < 2) {
    debPrint(" !!!! MBE TWO NO DATA !!!!");
    debPrintln("");
    debPrint("iCount OUT =");
    debPrintb(iCount);
    debPrintln("");
  }
  return bResponse;
}

const byte PIDMode0[8] = {2,1,0,0,0,0,0,0};
const byte PIDOBD[8] = {2,1,28,0,0,0,0,0}; 
const byte PIDMil[8] = {2,1,1,0,0,0,0,0};
const byte PIDSDTC[8] = {1,3,0,0,0,0,0,0};
const byte PIDPDTC[8] = {1,7,0,0,0,0,0,0};
const byte PIDMode9[8] = {2,9,0,0,0,0,0,0};
const byte PIDWater[8] = {2,1,5,0,0,0,0,0};
const byte PIDRPMH[8] = {2,1,12,0,0,0,0,0};
const byte PIDAdvance[8] = {2,1,14,0,0,0,0,0};
const byte PIDIAT[8] = {2,1,15,0,0,0,0,0};
const byte PIDTP[8] = {2,1,17,0,0,0,0,0};
const byte PIDBARO[8] = {2,1,51,0,0,0,0,0};

byte rpmLow = 0;
int rpmTotal;

void loop()
{

  if (digitalRead(0) == LOW) {
    debPrintln("Pin 0 is LOW");
  } else {
    debPrintln("Pin 0 is HIGH");
  }
  byte len = 0;
  byte buf[8];

  // ------------------- Supporting implemented OBD Requests
  // Get Water Temp from ECU
  bMBE_WATER = MBERequestOne(0xC0,bMBE_WATER);
  // Get RPM from ECU
  if (MBERequestTwo(0x7C,0xC3)) {
    bMBE_RPMLow = bMBEResponse1;
    bMBE_RPMHigh = bMBEResponse2;
  } else {
    //debPrintln("-NO RESPONSE");
  }
  // Get Ignition from ECU
  bMBE_IGN = MBERequestOne(0x87, bMBE_IGN);
  // Get Air Temp from ECU
  bMBE_AIR = MBERequestOne(0xBF,bMBE_AIR);  
  // Get Throttle position 1 from ECU
  bMBE_TP1 = MBERequestOne(0x9C,bMBE_TP1);
  // Get Baro from ECU
  bMBE_BARO = MBERequestOne(0x50,bMBE_BARO);

  // ------------------- Not relevant to OBD
  /*
  // Get Throttle position 2 from ECU
  bMBE_TP2 = MBERequestOne(0x94,bMBE_TP2);
  memcpy_P (&bOBD_TPOS2, &t_TPOBD[bMBE_TP2], sizeof bOBD_TPOS2);
  // Get Lambda 1 from ECU
  bMBE_LAM1 = MBERequestOne(0x49,bMBE_LAM1);
  // Get Lambda 2 from ECU
  bMBE_LAM2 = MBERequestOne(0x4A,bMBE_LAM2);
  // Get Adaptive 1 from ECU
  bMBE_ADAPT1 = MBERequestOne(0xEF,bMBE_ADAPT1);
  //Get Adaptive 2 from ECU
  bMBE_ADAPT2 = MBERequestOne(0xF1,bMBE_ADAPT2);
  // Get Battery from ECU
  bMBE_BATT = MBERequestOne(0xA7,bMBE_BATT);
  // Get Injector 1 from ECU
  if (MBERequestTwo(0x7E,0xC3)) {
    bMBE_INJ1Low = bMBEResponse1;
    bMBE_INJ1High = bMBEResponse2;
  } else {
    //debPrint("-NO RESPONSE");
  }
 // Get Injector 2 from ECU
  if (MBERequestTwo(0x76,0xC3)) {
    bMBE_INJ2Low = bMBEResponse1;
    bMBE_INJ2High = bMBEResponse2;
  } else {
    //debPrint("-NO RESPONSE");
  }
*/
  if (CAN_MSGAVAIL == CAN.checkReceive())           // check if data coming
  {
    CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

    // PIDs MODE 0 (message == "2,1,0,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDMode0, (const void *)buf, sizeof(buf)) == 0) {
      debPrintln("----- PIDs mode 0");
      send (6, 0x41, 0x00, 0x80, 0x10, 0x80, 0x10, 0x00);
    }
    // OBD Standard (message == "2,1,28,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDOBD, (const void *)buf, sizeof(buf)) == 0) {
      debPrintln("----- OBD Standard");
      send (3, 0x41, 0x1C, 0x0C, 0x00, 0x00, 0x00, 0x00);
    }
    // MIL Light (message == "2,1,1,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDMil, (const void *)buf, sizeof(buf)) == 0) {
      debPrintln("----- MIL Light");
      send(6, 0x41, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00);
    }
    // Mode 3, PID 0 (Stored DTC) (message == "1,3,0,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDSDTC, (const void *)buf, sizeof(buf)) == 0) {
      // Mode 3 PID 0
      debPrintln("----- Stored DTC request");
      send (4, 0x43, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00);
    }
    // Mode 7, PID 0 (pending DTC) (message == "1,7,0,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDPDTC, (const void *)buf, sizeof(buf)) == 0) {
    // MODE 7 PID 0
      debPrintln("----- Pending DTC");
      send (4, 0x47, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00);
    }
    //Pids Mode 9 (message == "2,9,0,0,0,0,0,0,") {
    if ( memcmp( (const void *)PIDMode9, (const void *)buf, sizeof(buf)) == 0) {
      //Serial.println("PIDs mode 9");
      debPrintln("----- Pids M9");
      send (6, 0x49, 0x00, 0xC0, 0xC0, 0x00, 0x00, 0x00);
    }
    // OBD Water temp  (message == "2,1,5,0,0,0,0,0,") {
    if ( memcmp( (const void *)PIDWater, (const void *)buf, sizeof(buf)) == 0) {
      // Water temp
      memcpy_P (&bOBD_ValueA, &t_mbeAIRWATER_OBD[bMBE_WATER], sizeof bOBD_ValueA);
      send (3, 0x41, 0x05, bOBD_ValueA, 0x00, 0x00, 0x00, 0x00); //
    }    
    //OBD RPM (message == "2,1,12,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDRPMH, (const void *)buf, sizeof(buf)) == 0) {
      memcpy_P (&rpmLow, &t_mbeRPMLow[bMBE_RPMLow], sizeof rpmLow);
      rpmTotal = rpmLow + 250*bMBE_RPMHigh;
      gtSerial.print(rpmTotal,DEC);
      // (256A + B)/4
      rpmTotal = rpmTotal * 4;
      bOBD_ValueA = rpmTotal / 256;
      bOBD_ValueB = rpmTotal - (bOBD_ValueA * 256);
      send (4, 0x41, 0x0C, bOBD_ValueA, bOBD_ValueB, 0x00, 0x00, 0x00); 
    }
    //OBD Timing Advance (message == "2,1,14,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDAdvance, (const void *)buf, sizeof(buf)) == 0) {
      // replace 0,0,0x00,0x00 with values below
      
      memcpy_P (&bOBD_ValueA, &t_TPIgnition_OBD[bMBE_IGN], sizeof bOBD_ValueA);
      send (4, 0x41, 0x0E, bOBD_ValueA, 0x00, 0x00, 0x00, 0x00); 
    }
    //OBD Intake air temp (message == "2,1,15,0,0,0,0,0,") 
    if ( memcmp( (const void *)PIDIAT, (const void *)buf, sizeof(buf)) == 0) {
      // replace 0,0,0x00,0x00 with values below
      memcpy_P (&bOBD_ValueA, &t_mbeAIRWATER_OBD[bMBE_AIR], sizeof bOBD_ValueA);
      send (4, 0x41, 0x0F, bOBD_ValueA, 0x00, 0x00, 0x00, 0x00); 
    }
    //OBD Throttle Position (message == "2,1,17,0,0,0,0,0,")
    if ( memcmp( (const void *)PIDTP, (const void *)buf, sizeof(buf)) == 0) {
      // Throttle position
      memcpy_P (&bOBD_ValueA, &t_TPOBD[bMBE_TP1], sizeof bOBD_ValueA);
      send (3, 0x41, 0x11, bOBD_ValueA, 0x00, 0x00, 0x00, 0x00); 
    }
    //OBD Barometric pressure (message == "2,1,51,0,0,0,0,0,")
    if ( memcmp( (const void *)PIDBARO, (const void *)buf, sizeof(buf)) == 0) {
      // Barometric pressure
      memcpy_P (&bOBD_ValueA, &t_mbeBARO_OBD[bMBE_BARO], sizeof bOBD_ValueA);
      send (3, 0x41, 0x33, bOBD_ValueA, 0x00, 0x00, 0x00, 0x00);
    }
  }
}

/*********************************************************************************************************
  END FILE
*********************************************************************************************************/