Diff: Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp

Revision:

34:18bcf276d3bf

Parent:

33:6bc82b6b62e5

Child:

38:8efacce315ae

diff -r 6bc82b6b62e5 -r 18bcf276d3bf Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp
--- a/Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,56 +1,93 @@
 #include "CAN-xbee.h"
 
+// Creates stringified version of CANMessage msg in char* buff (must be at least 15 chars)
+// Returns -1 for bad message, or else returns size of char* buff including the terminating '\n'
+int convert2array(CANMessage &msg, char* buff) {
+    
+    // Check message integrity
+    if (msg.len > 8) return -1;
+    if ((msg.format == CANStandard) && ((msg.id & 0x7FF) != msg.id)) return -1;
+    if ((msg.format == CANExtended) && ((msg.id & 0x1FFFFFFF) != msg.id)) return -1;
+    
+    int i = 0;
+    buff[i] = 'C';      // Start of message
+    i++;
+   /* buff[i] = (msg.format << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);     // Header byte, message info on ID size, RTR, # data bytes
+    i++;
+    if (msg.format == CANStandard) {
+        buff[i++] = (msg.id & 0x0FF);               // Lower byte of ID
+        buff[i++] = (msg.id & 0x700) >> 8;          // Upper byte of ID
+    } else {
+        buff[i++] = (msg.id & 0x000000FF);          // Lowest byte of ID
+        buff[i++] = (msg.id & 0x0000FF00) >> 8;
+        buff[i++] = (msg.id & 0x00FF0000) >> 16;
+        buff[i++] = (msg.id & 0x1F000000) >> 24;    // Highest byte of ID
+    }
+    for (int j = 0; j < msg.len; j++) {
+        buff[i++] = msg.data[j];
+    }
+    buff[i++] = '\n';
+    */return i;
+}
+
+bool convert2msg(CANMessage &msg, char* buff) {
+    // Check for 'D' for done
+    if (buff[0] != 'D') return -1;
+    
+    bool extended = buff[1] & 1;
+    bool rtr = buff[1] & 2;
+    char DLC = buff[1] >> 2;
+    if (DLC > 8) return false;     // Bad length
+    
+    // Standard frame, get id and data
+    if (!extended) {
+        if (buff[3] & 0x7 != buff[3]) return false;                 // Last byte of ID bad
+        msg.id = buff[2] | (int)(buff[3] << 8);                     // Build the ID
+        for (int i = 0; i < DLC; i++) msg.data[i] = buff[i+4];      // Build the data array
+        
+    // Extended frame, get id and data
+    } else {
+        if (buff[5] & 0x1F != buff[5]) return false;                            // Last byte of ID bad
+        msg.id = buff[2] | (buff[3] << 8) | (buff[4] << 16) | (buff[5] << 24);  // Build the ID
+        for (int i = 0; i < DLC; i++) msg.data[i] = buff[i+6];                  // Build the data array
+    }
+    msg.len = DLC;
+    if (rtr) msg.type = CANRemote;
+    else msg.type = CANData;
+    if (extended) msg.format = CANExtended;
+    else msg.format = CANStandard;
+    return true;                        // Successfully parsed, passed all checks, arguement was updated
+}
+
 CANxbee::CANxbee(PinName tx, PinName rx, int _baud, int txSize, int rxSize) : serial(tx, rx, txSize, rxSize) {
     serial.baud(_baud);
     rx_i = 0;
     rxBuff[0] = 0;
-    building = false;
+    getChars = -1;
 }
 
 // Send a CAN message, first reformat it into a char array, then send over Xbees
 bool CANxbee::send(CANMessage &msg) {
-    char buff[14];      // Build the string-ified CAN message here
-    int size;
-    
-    buff[0] = 'C';
-    int i;
-    if (msg.format == 0) {              // Standard, 11-bit ID
-        buff[1] = (0 << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);
-        buff[2] = msg.id & 0xFF;        // Lower byte of ID
-        buff[3] = (msg.id & 0x7FF) >> 8;
-        for (i = 0; (i < msg.len) && (i < 8); i++) {
-            buff[i+4] = msg.data[i];    // Get data bytes
-        }
-        buff[i+4] = '\n';               // Terminate message
-        size = i+4+1;
-        
-    } else if (msg.format == 1) {       // Extended, 29-bit ID
-        buff[1] = (1 << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);
-        buff[2] = msg.id & 0x000000FF;  // Lower byte of ID
-        buff[3] = msg.id & 0x0000FF00 >> 8;
-        buff[4] = msg.id & 0x00FF0000 >> 16;
-        buff[5] = msg.id & 0x1F000000 >> 24;
-        for (i = 0; (i < msg.len) && (i < 8); i++) {
-            buff[i+6] = msg.data[i];    // Get data bytes
-        }
-        buff[i+6] = '\n';               // Terminate message
-        size = i+6+1;
-    } else return false;                // Bad message
-
+    char buff[15];                      // Build the string-ified CAN message here
+    int size = convert2array(msg, buff);
+    if (size == -1) return false;       // Bad message, string not formed
     bool success=false;
     
-    // Begin thread-safe section
-    __disable_irq();
+    size=1;
+    int bytesLeft = serial.txBufferGetSize(0) - serial.txBufferGetCount();
+    Timer t;
     
-    // Check if enough spaces in buffer
-    if ((serial.txBufferGetSize(0) - serial.txBufferGetCount()) >= size) {
+    // Begin thread-safe section
+    NVIC_DisableIRQ(TIMER3_IRQn);       // Timer3-->RTOS tick
+
+    if (bytesLeft >= size) {
         for (int i = 0; i < size; i++) serial.putc(buff[i]);    // Send the message out
         success = true;
     }
     
     // End thread-safe section
-    __enable_irq();
-    
+    NVIC_EnableIRQ(TIMER3_IRQn);       // Timer3-->RTOS tick
+
     return success;
 }
 
@@ -59,62 +96,40 @@
     int newChar = serial.getcNb();
     if (newChar == -1) return false;        // No new char
     char c = newChar & 0xFF;                // Cast to char
-    bool process=false;
     
-    char len=0;
-    // Listen for a 'C', start of new message
-    if (c == 'C' && !building) {
-        rxBuff[rx_i] = 'C';
-        rx_i++;
-        building = true;    // Now building a string
+    // Listen for a 'C', start of new message as long as not already building message
+    if (c == 'C' && (rxBuff[0] != 'C')) {
+        rx_i = 0;               // Reset to start of message
+        rxBuff[rx_i++] = 'C';   // Add the 'C', increment
         return false;
     }
-    // Keep building a new message until too big or encounter '\n'
-    if (building) {
-        rxBuff[rx_i] = c;
-        rx_i++;
-        if (c == '\n') {        // Newline! process this string
-            process = true;
-            building = false;   // Reset to keep capturing on next call
-            len = rx_i;
-            rx_i = 0;
-        }
-        else if (rx_i >= sizeof(rxBuff)) {   // Too big, bad string
-            rx_i = 0;
-            building = false;
-            return false;
+    
+    // 'C' already found, now filling in contents of message
+    if (rxBuff[0] == 'C') {
+        if (getChars == -1) {           // Get the header byte
+            rxBuff[rx_i++] = c;         // Add to string, increment
+            bool extended = c & 1;      // Is this an extended message? (4 ID bytes)
+            char DLC = c >> 2;          // How long is the data section?
+            if (DLC > 8) {              // Bad DLC, reset variables
+                getChars = -1;
+                rxBuff[0] = 0;
+                rx_i = 0;
+                return false;
+            }
+            getChars = DLC + extended?4:2;  // Need to obtain getChars more characters to complete this message
+        } else {                            // This is not a header byte, add to contents of message
+            rxBuff[rx_i++] = c;
+            getChars--;
+            if (getChars <= 0) {            // Just added the last char, done message, reset variables
+                rxBuff[0] = 'D';            // Mark 'D' for done
+                rx_i = 0;
+                getChars = -1;
+            }
         }
     }
-    if (!process) return false;
+    if (rxBuff[0] != 'D') return false;
     
     // A string is ready, process it here
-    if (rxBuff[0] != 'C') return false;     // No start char, not valid
-    
-    // Extract the data from the header byte
-    bool extended = rxBuff[1] & 1;
-    bool rtr = rxBuff[1] & 2;
-    char DLC = rxBuff[1] >> 2;
-    int id=0;
-    if (DLC > 8) return false;                  // Bad DLC
-    if (!extended) {                            // Standard ID
-        if (len > 12 || len < 5) return false;  // Too big/too small for standard size
-        if (rxBuff[3] & 0x7 != rxBuff[3]) return false;     // Last byte of ID bad
-        if (len != 5 + DLC) return false;       // Improper number of bytes
-        id = rxBuff[2] | (rxBuff[3] << 8);      // Build the ID
-        for (int i = 0; i < DLC; i++) msg.data[i] = rxBuff[i+4];        // Build the data array
-
-    } else {                                    // Extended ID
-        if (len > 14 || len < 7) return false;  // Too big/too small for extended size
-        if (rxBuff[5] & 0x1F != rxBuff[5]) return false;                                // Last byte of ID bad
-        if (len != 7 + DLC) return false;       // Improper number of bytes
-        id = rxBuff[2] | (rxBuff[3] << 8) | (rxBuff[4] << 16) | (rxBuff[5] << 24);      // Build the ID
-        for (int i = 0; i < DLC; i++) msg.data[i] = rxBuff[i+6];        // Build the data array
-    }
-    msg.id = id;
-    msg.len = DLC;
-    if (rtr) msg.type = CANRemote;
-    else msg.type = CANData;
-    if (extended) msg.format = CANExtended;
-    else msg.format = CANStandard;
-    return true;            // Successfully parsed, passed all checks, arguement was updated
+    if (convert2msg(msg, rxBuff)) return true;
+    else return false;
 }
\ No newline at end of file