File content as of revision 0:1ce2190bc4d2:

#include "CANInterface.h"

CANInterface::CANInterface(PinName rd, PinName td) {
   _can = new CAN(rd,td);
   _can->frequency(CANInterface::CAN_SPEED_500);
   _timer = new Timer();
   _timeout = 200;
}

int CANInterface::makeRequest(uint8_t pid,  uint8_t *outBuf, int *outLen) {
   char canMsg[8] = {0x02,0x01,pid,0x00,0x00,0x00,0x00,0x00};
   int readOK = 0;
   
   // write request
   CANMessage request(CAN_PID_REQUEST,canMsg,8);
   if(_can->write(request)==0) {
      return 1;     
   }
   
   // get response
   CANMessage response;
   
   // start request timer
   _timer->reset();
   _timer->start();
   
   // wait for response
   while(_timer->read_ms() < _timeout) {
   
      // try and read response
      if(_can->read(response)) {
         readOK = 1;
         _timer->stop();
         if((response.id == CAN_PID_REPLY) && (response.data[2] == pid)) {
            memcpy(outBuf,response.data,response.len-2);
            outBuf[0] = response.data[3];
            outBuf[1] = response.data[4];
            *outLen = response.len-2;
            break;
         }
      }
   }
   return readOK;
}