Utilizing OEM’s GPS for embedded Navigation tasks with the Teensy 4.1

RichW

New member
Utilizing OEM’s GPS for embedded Navigation tasks with the Teensy 4.1

The Teensy 4.1 embeds all of the power and functionality I have been looking for into a low cost and compact development platform for my vehicle. I utilize the GPS that is already built into my GM Chevrolet 2016 Equinox to simplify installation and minimize the cost by way of tapping into the CAN bus via the OBD-II connector. Most GM vehicles with Onstar would likely have their GPS data available as well thru the built in OBD-II connector. To demonstrate and help develop the system I used the built-in CAN, USB and micro SD card hardware available to the Teensy 4.1 (T4.1) controller to develop this and my CAN logger using the same hardware that was described here:

https://forum.pjrc.com/threads/62705-Utilizing-a-Teensy-4-1-for-an-in-vehicle-CAN-data-logger

For my first GPS implementation I wanted to demonstrate that I can acquire the required CAN bus signals and translate to a common $GPRMC NEMA format that can be logged to the micro SD card and post processed by Google Earth on my pc to help develop future functionality.

The OBD-II port has one standard pin pair for a single CAN bus, but many automakers expose other buses on additional, non-standard pin pairs. For this GM 2016 Equinox implementation, the CAN bus uses 11 bit CAN ID’s and a 500kb baud rate. Other vehicles may use different CAN setups. I’ll use this nomenclature:

OBD-II Pin Pair OpenXC Bus Name Other common name
6 (+) and 14 (-) CAN1 CAN high / CAN low or CAN + / CAN –

Below is my T4.1 breadboard setup with optional power LED and test points that are not required. The only additional hardware required is the CAN vehicle transceiver to the upper right of the T4.1 board and a cable to the vehicles OBDII connector besides a micro SD card. The micro SD card I am using is an older unnamed 8 GB card I had laying around that seems to work ok.

CanGpsHw.JPG

I like to use a low cost and easy to find DB9 male connector on the T4.1 end that connects to a DB9F and then the harder to find and more expensive OBDII connector for the more in place vehicle harness. This also helps with other CAN devices I use on my vehicle but you can use what works easiest for you to connect your T4.1 to your vehicle. This also helps me with taking my T4.1 in and out of my car for office development as well. Below is a schematic of my breadboard, minus the optional power LED indicator and test points. I can use the built in T4.1 LED for software debug now that I am confident of the input power. I use an easily available 12V cigarette to USB power adapter to power the T4.1. This is the easiest for me the way my 2016 Equinox is setup. Most vehicles nowadays supply some USB access port to power the T4.1 which helps simplify development hardware.
The Arduino software implements the FlexCAN_T4 library which can be installed from here:

https://github.com/tonton81

The Arduino software also implements TeensyTimerTool which is here:

https://github.com/luni64/TeensyTimerTool

CanSD_Schematic.JPG

Below is a snapshot that demonstrates the software functionality that logs the OEM’s factory installed GPS signals of my vehicle to the micro SD card for analysis on Google Earth. This is done by using the File -> Open command on Google Earth and selecting the GPSxx.log file that was logged on the micro SD card. The speed and elevation profiles can be added by the Edit -> Show Elevation Profile

GoogleEarthSnap2.JPG

My future development efforts are to add a driver aide to help with safety and maximize fuel economy of my most commonly used vehicle routes which is why I developed the Google Earth interface. This also will require a screen such as this:

https://www.pjrc.com/store/display_ili9341_touch.html

Other development ideas are to perhaps support additional vehicles since I have a 2016 Ford Escape as well in my vehicle development fleet.


Below is a sample log file.

View attachment GPS02.zip

Below is the code.

Code:
//
// Beta CanGpsSD Beta 1.0 
// This version implements baseline functionality 8/26/2020
// 
// Uses Library
// https://github.com/tonton81
// C:\Program Files (x86)\Arduino\hardware\teensy\avr\libraries\FlexCAN_T4
//
// include FlexCAN_T4FD if you want CANFD
// 
// 
//
#include <FlexCAN_T4.h>
FlexCAN_T4<CAN1, RX_SIZE_16, TX_SIZE_16> Can1; //orig RX_SIZE_256 TX_SIZE_64

byte NEMA_xsum;
char NEMA_str[128] = "$GPRMC,\0\0";
char Year_str[3] = "01";
char Month_str[3] = "01";
char Day_str[3] = "01";
char Hour_str[3] = "01";
char Min_str[3] = "01";
char Sec_str[3] = "01";

char VehLatInt_str[8] = "00.00\0\0";
char VehLonInt_str[8] = "000.00\0";  

char VehLatMin_str[8] = "00.00\0\0";
char VehLonMin_str[8] = "00.00\0\0";  

char VehSpd_str[8] = "0";
char VehHdg_str[8] = "  0.0\0\0";  

char NEMA_xsum_str[4] = "00";
  
byte Engine_Run_Active = 0;
byte Last_Engine_Run_Active = 0;

byte hrs_valid = 0;
byte min_valid = 0;
byte sec_valid = 0;

 /* Variables for 0x135, 0x137, 0x32A */
unsigned int year = 2020;
byte  VePPSR_y_Year = 20;
byte  VePPSR_y_Month = 7;
byte  VePPSR_y_Day = 30;
byte  VePPSR_y_Hour = 22;  
byte  VePPSR_y_Min = 11;
byte  VePPSR_y_Sec = 9;

byte  VePPSR_y_Lat_Valid = 0;
byte  VePPSR_y_Long_Valid = 0;
byte  VePPSR_y_NewMsg = 0;

int   VePPSR_l_VehLatInt = 0;
int   VePPSR_l_VehLonInt = 0;

float VeECMR_v_VehSpd = 0.0;
float VePPSR_l_PDOP = 0.0;  
float VePPSR_l_VehLat = 0.0;
float VePPSR_l_VehLon = 0.0;
float VePPSR_l_VehLatMin = 0.0;
float VePPSR_l_VehLonMin = 0.0;

float VePPSR_d_VehHdg = 0.0;
float VePPSR_l_VehAlt = 0.0;

union CanMsg
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} CanMsg_Data;

union Msg_0x135
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x135_Data;

union Msg_0x135_Last
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x135_Data_Last;

union Msg_0x137
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
      
} Msg_0x137_Data;

union Msg_0x137_Last
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x137_Data_Last;

union Msg_0x32A
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x32A_Data;

union Msg_0x32A_Last
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x32A_Data_Last;

union Msg_0x3E9
{
  unsigned char ub[8];
  unsigned int  ui[4];
  unsigned long ul[2];
  signed long   sl[2];  
 
} Msg_0x3E9_Data;

//
//
#include <SD.h>
#include <SPI.h>

File myFile;
char fn[12] = "gps00.log";
char File_Opened = 0;
    
// 
// 

/******************************************************************************
Function Name : uitoa
Engineer    : raw 
Date        : 07/25/2008
Parameters  : value to convert, string
Returns     : length of string
Notes       : convert unsigned int to string
******************************************************************************/

unsigned int uitoa(unsigned int n, char s[])
{
  unsigned int i = 0;
  do {
     ++i;
     *s-- = n % 10 + '0';
  } while ((n /= 10) > 0);

  return(i);
}

#include "TeensyTimerTool.h"

using namespace TeensyTimerTool;

Timer t1; // generate a timer from the pool (Pool: 2xGPT, 16xTMR(QUAD), 20xTCK)
void T1_Callback()
{
  digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));    
  if (VePPSR_y_NewMsg) 
  {    
    VePPSR_y_NewMsg = 0;
    
    uitoa((unsigned int)VePPSR_y_Hour, &Hour_str[1]);
    Hour_str[2] = 0;
    if (VePPSR_y_Hour < 10)
    {
      Hour_str[0] = '0';
    } 
  
    uitoa((unsigned int)VePPSR_y_Min, &Min_str[1]);
    Min_str[2] = 0;
    if (VePPSR_y_Min < 10)
    {
      Min_str[0] = '0';
    }

    uitoa((unsigned int)VePPSR_y_Sec, &Sec_str[1]);
    Sec_str[2] = 0;
    if (VePPSR_y_Sec < 10)
    {
      Sec_str[0] = '0';
    }

    uitoa((unsigned int)VePPSR_y_Year, &Year_str[1]);
    Year_str[2] = 0;
    if (VePPSR_y_Year < 10)
    {
      Year_str[0] = '0';    
    }

    uitoa((unsigned int)VePPSR_y_Month, &Month_str[1]);
    Month_str[2] = 0;   
    if (VePPSR_y_Month < 10)
    {    
      Month_str[0] = '0';   
    }

    uitoa((unsigned int)VePPSR_y_Day, &Day_str[1]);
    Day_str[2] = 0;    
    if (VePPSR_y_Day < 10)
    {
      Day_str[0] = '0';    
    }

    // NEMA Start
    strcpy(&NEMA_str[0], "$GPRMC,\0");
    
    strcat(NEMA_str, Hour_str); 
    strcat(NEMA_str, Min_str); 
    strcat(NEMA_str, Sec_str);  
   
    if (VePPSR_y_Lat_Valid || VePPSR_y_Long_Valid) // 0 = valid
    {
      strcat (NEMA_str, ",V,"); // 1 = invalid
    } else {
      strcat (NEMA_str, ",A,"); // 0 = valid
    }

    VehLatInt_str[0] = '0';
    VehLatInt_str[1] = '0';
    VehLatInt_str[2] = '\0';
    VehLatInt_str[3] = '\0';
    
    VehLatMin_str[0] = '0';
    VehLatMin_str[1] = '0';
    VehLatMin_str[2] = '.';
    VehLatMin_str[3] = '0';
    VehLatMin_str[4] = '0';    
    VehLatMin_str[5] = '\0';

    if (VePPSR_l_VehLat < 0.0)
    {
      uitoa(-VePPSR_l_VehLatInt, &VehLatInt_str[1]);   
      strcat (NEMA_str, VehLatInt_str);
  
      dtostrf(VePPSR_l_VehLatMin, 6, 4, VehLatMin_str);
      if (VePPSR_l_VehLatMin < 10.0) {
        VehLatMin_str[0] = '0';            
      }
          
      strcat (NEMA_str, VehLatMin_str);              
      strcat(NEMA_str, ",S,"); // S = negative
    }
    
    else {
      uitoa(VePPSR_l_VehLatInt, &VehLatInt_str[1]);   
      strcat (NEMA_str, VehLatInt_str);
  
      dtostrf(VePPSR_l_VehLatMin, 6, 4, VehLatMin_str); 
      if (VePPSR_l_VehLatMin < 10.0) {
        VehLatMin_str[0] = '0';            
      }    
           
      strcat (NEMA_str, VehLatMin_str);
      strcat(NEMA_str, ",N,"); // N = positive     
    }
            
    VehLonInt_str[0] = '0';
    VehLonInt_str[1] = '0';
    VehLonInt_str[2] = '0';
    VehLonInt_str[3] = '\0';
    
    VehLonMin_str[0] = '0';
    VehLonMin_str[1] = '0';
    VehLonMin_str[2] = '.';
    VehLonMin_str[3] = '0';
    VehLonMin_str[4] = '0';   
    VehLonMin_str[5] = '\0';
    
    if (VePPSR_l_VehLon < 0.0)
    {
      uitoa(-VePPSR_l_VehLonInt, &VehLonInt_str[2]);   
      strcat(NEMA_str, VehLonInt_str); 
      dtostrf(VePPSR_l_VehLonMin, 7, 4, VehLonMin_str);
      if (VePPSR_l_VehLonMin < 10.0) {
        VehLonMin_str[0] = '0';            
      }
      strcat(NEMA_str, VehLonMin_str);                         
      strcat(NEMA_str, ",W,"); // W = negative 
    }
    
    else {
      uitoa(VePPSR_l_VehLonInt, &VehLonInt_str[2]);   
      strcat(NEMA_str, VehLonInt_str);
      dtostrf(VePPSR_l_VehLonMin, 7, 4, VehLonMin_str); 
      if (VePPSR_l_VehLonMin < 10.0) {
        VehLonMin_str[0] = '0';            
      }      
        
      strcat(NEMA_str, VehLonMin_str);
      strcat(NEMA_str, ",E,"); // E = positive         
    }

    if (VeECMR_v_VehSpd < 10.0){
      dtostrf(VeECMR_v_VehSpd, 3, 1, VehSpd_str); // in knots already   
    }
    
    else if (VeECMR_v_VehSpd < 100.0) {
      dtostrf(VeECMR_v_VehSpd, 4, 1, VehSpd_str); // in knots already
    }

    else {
      dtostrf(VeECMR_v_VehSpd, 5, 1, VehSpd_str); // in knots already   
    }

    if (VePPSR_d_VehHdg < 10.0) {
      dtostrf(VePPSR_d_VehHdg, 3, 1, VehHdg_str);
    }

    else if (VePPSR_d_VehHdg < 100.0) {
      dtostrf(VePPSR_d_VehHdg, 4, 1, VehHdg_str);
    }

    else {
      dtostrf(VePPSR_d_VehHdg, 5, 1, VehHdg_str);
    }
    
    if (VeECMR_v_VehSpd > 0.0)
    {
      strcat(NEMA_str, VehSpd_str);
    } 
    
    else {
      strcat(NEMA_str, "0.0");      
    }
       
    strcat(NEMA_str, ",");
    
    if (VePPSR_d_VehHdg > 0.0)
    {
       strcat(NEMA_str, VehHdg_str);
    } 
    
    else {
      strcat(NEMA_str, "0.0");      
    }
    
    strcat(NEMA_str, ",");

    strcat(NEMA_str, Day_str);
    strcat(NEMA_str, Month_str);
    strcat(NEMA_str, Year_str);
    strcat(NEMA_str, ",0.0,W,A*");

    byte i = 1;
    NEMA_xsum = 0;
    while (i < 80 && NEMA_str[i] != '*' && NEMA_str[i] != '\0') {
      NEMA_xsum ^= NEMA_str[i];
      i++;
    }
    
   ultoa(NEMA_xsum, NEMA_xsum_str,16);
   if (NEMA_xsum < 16){
     strcat(NEMA_str, "0");  
     }
    strcat(NEMA_str, NEMA_xsum_str);   
    strcat(NEMA_str, "\r\n");
     
    Serial.println(NEMA_str);
     
    if (File_Opened > 0){
      myFile.print(NEMA_str);
    }
  }
}
     
void setup(void) {
  Serial.begin(115200); //delay(400);
  pinMode(6, OUTPUT); digitalWrite(6, LOW); /* optional tranceiver enable pin */

  //  Unions need to be initialized inside a routine not where defined!
  Msg_0x135_Data_Last.ul[0] = 0;
  Msg_0x135_Data_Last.ul[1] = 0;

  Msg_0x137_Data_Last.ul[0] = 0;
  Msg_0x137_Data_Last.ul[1] = 0;

  Msg_0x32A_Data_Last.ul[0] = 0;
  Msg_0x32A_Data_Last.ul[1] = 0;
  
  Can1.begin();
  Can1.setBaudRate(500000);
  Can1.setMaxMB(16);
  Can1.enableFIFO();
  Can1.enableFIFOInterrupt();
  Can1.onReceive(canSniff);
  Can1.mailboxStatus();
  Can1.enableMBInterrupts();
  pinMode(13, OUTPUT);
  
//Setup Periodic Timer 
  pinMode(LED_BUILTIN,OUTPUT);  
  t1.beginPeriodic(T1_Callback, 50'000); // 50ms 

// Set time date stamp.
  SdFile::dateTimeCallback(dateTime);

  Serial.print(F("Initializing SD card..."));

  if (!SD.begin(BUILTIN_SDCARD)) {
    Serial.println(F("initialization failed!"));
    return;
  }
  Serial.println(F("initialization done."));
  
  while ((SD.exists(fn)) && (((fn[4] <= 0x39) || (fn[4] < 0x5A)))) { // 0x39 = "9"
    fn[4]++;
    if (fn[4] == 0x3A){
      fn[4] = 0x41; //"A"
    }
    else if (fn[4] >= 0x5A){
      fn[4] = 0x5A; //"Z"
    }
  }

  // Wait here a few seconds to see if a valid time date stamp exists then move on
 while ( (millis() < 10000) && (hrs_valid == 1 && min_valid == 1 && sec_valid == 1)){}

  // open the file. 
  myFile = SD.open(fn, FILE_WRITE);
  
  // if the file opened okay, write to it:
  if (myFile) {
    File_Opened = 1;
    Serial.print(F("Writing to gpsXX.log..."));
    Serial.println(F("done."));
  } else {
    // if the file didn't open, print an error:
    Serial.println(F("error opening testx.txt"));
  }  
}

//Callback routine for the timedate stamp of the file
void dateTime(uint16_t* date, uint16_t* time)
{
  *date = FAT_DATE(year, VePPSR_y_Month, VePPSR_y_Day);
  *time = FAT_TIME(VePPSR_y_Hour, VePPSR_y_Min, VePPSR_y_Sec);
}

void RecCANID (const CAN_message_t msg) { 

  unsigned int Msg_Data_uint = 0;
  
  switch (msg.id) {

    case 0xC9:
      // Engine Run Active MSB byte 0 ie. 0x80 12 ms
      if (((msg.buf[0] & 0x80) == 0x00) && (Last_Engine_Run_Active == 0x80)) {
        File_Opened = 0;
       
        myFile.close();
        Serial.println("File closed.");
        Serial.println(msg.buf[0]);
      }
      
      Last_Engine_Run_Active =  msg.buf[0] & 0x80;
              
      break;
    
    case 0x135:
      // Wiper, ddmmyr,secs 100ms
      // 3,2,1,0,7,6,5,4 was the math order
      Msg_0x135_Data.ub[0] = msg.buf[0];
      Msg_0x135_Data.ub[1] = msg.buf[1];
      Msg_0x135_Data.ub[2] = msg.buf[2];
      Msg_0x135_Data.ub[3] = msg.buf[3];
      Msg_0x135_Data.ub[4] = msg.buf[4];
      Msg_0x135_Data.ub[5] = msg.buf[5];
      Msg_0x135_Data.ub[6] = msg.buf[6];
      Msg_0x135_Data.ub[7] = msg.buf[7];

      VePPSR_y_Day = Msg_0x135_Data.ub[2] & 0x1F;
      VePPSR_y_Month = Msg_0x135_Data.ub[4] & 0x0F;
      VePPSR_y_Year = Msg_0x135_Data.ub[5];
      if ((Msg_0x135_Data.ub[7] & 0x40) == 0) {
        VePPSR_y_Sec = Msg_0x135_Data.ub[7] & 0x3F;
        sec_valid = 1;
      } else {
        sec_valid = 0;
      }
      
      break;
      
    case 0x137:
      // Minutes, heading, elev, hours, pdop 100ms
      // 3,2,1,0,7,6,5,4 was the math order      
      Msg_0x137_Data.ub[0] = msg.buf[0];
      Msg_0x137_Data.ub[1] = msg.buf[1];
      Msg_0x137_Data.ub[2] = msg.buf[2];
      Msg_0x137_Data.ub[3] = msg.buf[3];
      Msg_0x137_Data.ub[4] = msg.buf[4];
      Msg_0x137_Data.ub[5] = msg.buf[5];
      Msg_0x137_Data.ub[6] = msg.buf[6];
      Msg_0x137_Data.ub[7] = msg.buf[7];


      if (Msg_0x137_Data.ul[0] != Msg_0x137_Data_Last.ul[0] || 
          Msg_0x137_Data.ul[1] != Msg_0x137_Data_Last.ul[1]) {
        

        if ((Msg_0x137_Data.ub[1] & 0x80) == 0) {
          VePPSR_y_Min = Msg_0x137_Data.ub[0] & 0x3F;
          min_valid = 1;
        } else{
          min_valid = 0;
        }

        if ((Msg_0x137_Data.ub[1] & 0x40) == 0) {
          hrs_valid = 1;
          VePPSR_y_Hour = ((Msg_0x137_Data.ub[6] & 0x7C) >> 2); // -4 = EST
        } else {
          hrs_valid = 0;
        }
        
        if ((Msg_0x137_Data.ub[1] & 0x20) == 0) {
          Msg_Data_uint = ((unsigned int) (Msg_0x137_Data.ub[4]) << 8) + Msg_0x137_Data.ub[5];
          VePPSR_d_VehHdg = (Msg_Data_uint & 0x0FFF) * 0.1;
        }

        if ((Msg_0x137_Data.ub[0] & 0x40) == 0) {
          Msg_Data_uint = ((unsigned int) (Msg_0x137_Data.ub[6]) << 8) + Msg_0x137_Data.ub[7];          
          VePPSR_l_PDOP = (Msg_Data_uint & 0x03FF) * 0.1;
        }       
      }
      
      Msg_0x137_Data_Last.ul[0] = Msg_0x137_Data.ul[0];
      Msg_0x137_Data_Last.ul[1] = Msg_0x137_Data.ul[1];

      break;
      
    case 0x32A:
      // GPS 100ms
      // 3,2,1,0,7,6,5,4 is the little endian math order      
      
      Msg_0x32A_Data.ub[0] = msg.buf[3];
      Msg_0x32A_Data.ub[1] = msg.buf[2];
      Msg_0x32A_Data.ub[2] = msg.buf[1];
      Msg_0x32A_Data.ub[3] = msg.buf[0];
      Msg_0x32A_Data.ub[4] = msg.buf[7];
      Msg_0x32A_Data.ub[5] = msg.buf[6];
      Msg_0x32A_Data.ub[6] = msg.buf[5];
      Msg_0x32A_Data.ub[7] = msg.buf[4];

      VePPSR_y_NewMsg = 1;
      
      if (Msg_0x32A_Data.ul[0] != Msg_0x32A_Data_Last.ul[0] || 
          Msg_0x32A_Data.ul[1] != Msg_0x32A_Data_Last.ul[1] ) {
            
        CanMsg_Data.ul[0] = Msg_0x32A_Data.ul[0];
        CanMsg_Data.ul[1] = Msg_0x32A_Data.ul[1];
        //note Msg_0x32A_Data.ub[3] = msg.buf[0] from above so really byte 0 because of little endian
        if ((Msg_0x32A_Data.ub[3] & 0x40) == 0) {
          //Get the sign 
          CanMsg_Data.sl[0] = CanMsg_Data.sl[0] << 2;
          CanMsg_Data.sl[0] = CanMsg_Data.sl[0] >> 2;
          
          VePPSR_l_VehLat = 2.777778e-07 * CanMsg_Data.sl[0];

        }
 
        //note Msg_0x32A_Data.ub[7] = msg.buf[4] from above so really byte 4 because of little endian 
        if ((Msg_0x32A_Data.ub[7] & 0x80) == 0) {
          //Get the sign 
          CanMsg_Data.sl[1] = CanMsg_Data.sl[1] << 1;
          CanMsg_Data.sl[1] = CanMsg_Data.sl[1] >> 1;

          VePPSR_l_VehLon = 2.777778E-07 * CanMsg_Data.sl[1];
        }
        
        VePPSR_l_VehLatInt = (int) (VePPSR_l_VehLat);
        VePPSR_l_VehLonInt = (int) (VePPSR_l_VehLon);
        
        VePPSR_l_VehLatMin = fabs(VePPSR_l_VehLat - (int) (VePPSR_l_VehLat)) * 60.0; //Degrees MM.MM
        VePPSR_l_VehLonMin = fabs(VePPSR_l_VehLon - (int) (VePPSR_l_VehLon)) * 60.0;
        
        VePPSR_y_Lat_Valid  = (Msg_0x32A_Data.ub[3] & 0x40);
        VePPSR_y_Long_Valid = (Msg_0x32A_Data.ub[7] & 0x80); 
      }

      Msg_0x32A_Data_Last.ul[0] = Msg_0x32A_Data.ul[0];
      Msg_0x32A_Data_Last.ul[1] = Msg_0x32A_Data.ul[1];
      
      break;

    case 0x3E9:
      // Vspd, distance traveled
      
      Msg_0x3E9_Data.ub[0] = msg.buf[0];
      Msg_0x3E9_Data.ub[1] = msg.buf[1];
      Msg_0x3E9_Data.ub[2] = msg.buf[2];
      Msg_0x3E9_Data.ub[3] = msg.buf[3];
      Msg_0x3E9_Data.ub[4] = msg.buf[4];
      Msg_0x3E9_Data.ub[5] = msg.buf[5];
      Msg_0x3E9_Data.ub[6] = msg.buf[6];
      Msg_0x3E9_Data.ub[7] = msg.buf[7];

      if ((Msg_0x3E9_Data.ub[4] & 0x80) == 0) {
        Msg_Data_uint = ((unsigned int) (Msg_0x3E9_Data.ub[4]) << 8) + Msg_0x3E9_Data.ub[5];
        VeECMR_v_VehSpd = Msg_Data_uint  * 0.008436825; // msb will be zero if valid   
      } 
 
    break;
    
    default:
      // nothing matches

    break;
   }  

}

//CAN interrupt Callback function
void canSniff(const CAN_message_t &msg) { 

//            1         2         3         4           5
//  012345678901234567890123456789012345678901234567 8 90 
// "      0.000 1 000 Rx d 8 00 00 00 00 00 00 00 00\r\n";

  RecCANID (msg);
  
}

void loop() {
}
 
Back
Top