Diff: CANInterface.cpp

Revision:

0:1ce2190bc4d2

Child:

1:abf508583871

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CANInterface.cpp	Wed Apr 30 13:51:22 2014 +0000
@@ -0,0 +1,44 @@
+#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;
+}
\ No newline at end of file