1 files changed, 134 insertions, 0 deletions
diff --git a/source/packetParser.cpp b/source/packetParser.cpp
new file mode 100755
index 0000000..8f0194f
--- /dev/null
+++ b/source/packetParser.cpp
@@ -0,0 +1,134 @@
+#include <string.h>
+#include <Arduino.h>
+#include <bluefruit.h>
+
+
+#define PACKET_ACC_LEN                  (15)
+#define PACKET_GYRO_LEN                 (15)
+#define PACKET_MAG_LEN                  (15)
+#define PACKET_QUAT_LEN                 (19)
+#define PACKET_BUTTON_LEN               (5)
+#define PACKET_COLOR_LEN                (6)
+#define PACKET_LOCATION_LEN             (15)
+
+//    READ_BUFSIZE            Size of the read buffer for incoming packets
+#define READ_BUFSIZE                    (20)
+
+
+/* Buffer to hold incoming characters */
+uint8_t packetbuffer[READ_BUFSIZE+1];
+
+/**************************************************************************/
+/*!
+    @brief  Casts the four bytes at the specified address to a float
+*/
+/**************************************************************************/
+float parsefloat(uint8_t *buffer) 
+{
+  float f;
+  memcpy(&f, buffer, 4);
+  return f;
+}
+
+/**************************************************************************/
+/*! 
+    @brief  Prints a hexadecimal value in plain characters
+    @param  data      Pointer to the byte data
+    @param  numBytes  Data length in bytes
+*/
+/**************************************************************************/
+void printHex(const uint8_t * data, const uint32_t numBytes)
+{
+  uint32_t szPos;
+  for (szPos=0; szPos < numBytes; szPos++) 
+  {
+    Serial.print(F("0x"));
+    // Append leading 0 for small values
+    if (data[szPos] <= 0xF)
+    {
+      Serial.print(F("0"));
+      Serial.print(data[szPos] & 0xf, HEX);
+    }
+    else
+    {
+      Serial.print(data[szPos] & 0xff, HEX);
+    }
+    // Add a trailing space if appropriate
+    if ((numBytes > 1) && (szPos != numBytes - 1))
+    {
+      Serial.print(F(" "));
+    }
+  }
+  Serial.println();
+}
+
+/**************************************************************************/
+/*!
+    @brief  Waits for incoming data and parses it
+*/
+/**************************************************************************/
+uint8_t readPacket(BLEUart *ble_uart, uint16_t timeout) 
+{
+  uint16_t origtimeout = timeout, replyidx = 0;
+
+  memset(packetbuffer, 0, READ_BUFSIZE);
+
+  while (timeout--) {
+    if (replyidx >= 20) break;
+    if ((packetbuffer[1] == 'A') && (replyidx == PACKET_ACC_LEN))
+      break;
+    if ((packetbuffer[1] == 'G') && (replyidx == PACKET_GYRO_LEN))
+      break;
+    if ((packetbuffer[1] == 'M') && (replyidx == PACKET_MAG_LEN))
+      break;
+    if ((packetbuffer[1] == 'Q') && (replyidx == PACKET_QUAT_LEN))
+      break;
+    if ((packetbuffer[1] == 'B') && (replyidx == PACKET_BUTTON_LEN))
+      break;
+    if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN))
+      break;
+    if ((packetbuffer[1] == 'L') && (replyidx == PACKET_LOCATION_LEN))
+      break;
+
+    while (ble_uart->available()) {
+      char c =  ble_uart->read();
+      if (c == '!') {
+        replyidx = 0;
+      }
+      packetbuffer[replyidx] = c;
+      replyidx++;
+      timeout = origtimeout;
+    }
+    
+    if (timeout == 0) break;
+    delay(1);
+  }
+
+  packetbuffer[replyidx] = 0;  // null term
+
+  if (!replyidx)  // no data or timeout 
+    return 0;
+  if (packetbuffer[0] != '!')  // doesn't start with '!' packet beginning
+    return 0;
+  
+  // check checksum!
+  uint8_t xsum = 0;
+  uint8_t checksum = packetbuffer[replyidx-1];
+  
+  for (uint8_t i=0; i<replyidx-1; i++) {
+    xsum += packetbuffer[i];
+  }
+  xsum = ~xsum;
+
+  // Throw an error message if the checksum's don't match
+  if (xsum != checksum)
+  {
+    Serial.print("Checksum mismatch in packet : ");
+    printHex(packetbuffer, replyidx+1);
+    return 0;
+  }
+  
+  // checksum passed!
+  return replyidx;
+}
+