RV-C__CAN_Bus_Monitor.ino

/*
 * CAN_Bus_Monitor
 *   by: Michael J. Kidd <linuxkidd@gmail.com>
 *  Rev: 1.0
 * Date: 2015-11-23
 * 
 */

#include <SPI.h>
#include "mcp_can.h"
INT32U canId = 0x000;

unsigned char len = 0;
unsigned char buf[8];
char str[20];

// 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 = 10;

MCP_CAN CAN(SPI_CS_PIN);     // Set CS pin

void setup()
{    
    Serial.begin(230400);

START_INIT:
    if(CAN_OK == CAN.begin(CAN_250KBPS))
    {
//        Serial.println("CAN BUS Shield init ok!");
//        Serial.print("Resetting Filters...");
        CAN.init_Mask(0,1,0);
        CAN.init_Mask(1,1,0);
        for(int i=0;i<6;i++) {
          CAN.init_Filt(i,1,0);
        }
//        Serial.println("Done");
        Serial.println("millis,prio,dgn,srcaddr,bytes,contents");
    }
    else
    {
//        Serial.println("CAN BUS Shield init fail");
//        Serial.println("Init CAN BUS Shield again");
        delay(100);
        goto START_INIT;
    }
}

void loop()
{
    if(CAN_MSGAVAIL == CAN.checkReceive())
    {
        char outbuf[8];
        char hexPrio[8];
        char hexDGN[8];
        char hexDGNHI[8];
        char hexDGNLO[8];
        char hexSrcAD[8];
        CAN.readMsgBuf(&len, buf);
        canId = CAN.getCanId();

        String binCanID = String(canId,BIN);
        String prio  = binCanID.substring( 0, 3);
        String dgn   = binCanID.substring( 4,21);
/*        
        String dgnHI = binCanID.substring( 4,13);
        String dgnLO = binCanID.substring(13,21);
*/

        String srcAD = binCanID.substring(21,30);

        sprintf(hexPrio,"%X",reinterpret_cast<const char*>(bintoint(prio)));
        sprintf(hexDGN,"%04X",reinterpret_cast<const char*>(bintoint(dgn)));
 /*       
        sprintf(hexDGNHI,"%03X",reinterpret_cast<const char*>(bintoint(dgnHI)));
        sprintf(hexDGNLO,"%02X",reinterpret_cast<const char*>(bintoint(dgnLO)));
*/
        sprintf(hexSrcAD,"%02X",reinterpret_cast<const char*>(bintoint(srcAD)));

        Serial.print(millis());Serial.print(",");
        Serial.print(hexPrio);Serial.print(",");
        // Flaw in 'bintoint' code prevents the leading 1 of DGN from being properly coded.
        // Adding it in on the leading edge works well
        Serial.print(dgn.substring(0,1));Serial.print(hexDGN);Serial.print(",");
/*
        Serial.print(hexDGNHI);Serial.print(",");
        Serial.print(hexDGNLO);Serial.print(",");
*/
        Serial.print(hexSrcAD);Serial.print(",");
        Serial.print(len);Serial.print(",");
        for(int i = 0; i<len; i++)
        {
            sprintf(outbuf,"%02X",reinterpret_cast<const char*>(buf[i]));
            Serial.print(outbuf);
        }
        Serial.println();
   }
}

unsigned int bintoint(String strDigits){
  const char * digits = strDigits.c_str();
  INT32U res=0;
  while(*digits)
    res = (res<<1)|(*digits++ -'0');

  return res;
  
}

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