added basic code

2 files changed, 318 insertions, 0 deletions

diff --git a/source/controller.cpp b/source/controller.cpp
new file mode 100755
index 0000000..f6393e8
--- /dev/null
+++ b/source/controller.cpp
@@ -0,0 +1,184 @@
+/*********************************************************************
+ This is an example for our nRF52 based Bluefruit LE modules
+
+ Pick one up today in the adafruit shop!
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit and open-source hardware by purchasing
+ products from Adafruit!
+
+ MIT license, check LICENSE for more information
+ All text above, and the splash screen below must be included in
+ any redistribution
+*********************************************************************/
+
+#include <bluefruit.h>
+
+BLEUart bleuart;
+
+// Function prototypes for packetparser.cpp
+uint8_t readPacket (BLEUart *ble_uart, uint16_t timeout);
+float   parsefloat (uint8_t *buffer);
+void    printHex   (const uint8_t * data, const uint32_t numBytes);
+
+// Packet buffer
+extern uint8_t packetbuffer[];
+
+void startAdv(void);
+
+void setup(void)
+{
+  Serial.begin(115200);
+  while ( !Serial ) delay(10);   // for nrf52840 with native usb
+
+  Serial.println(F("Adafruit Bluefruit52 Controller App Example"));
+  Serial.println(F("-------------------------------------------"));
+
+  Bluefruit.begin();
+  // Set max power. Accepted values are: -40, -30, -20, -16, -12, -8, -4, 0, 4
+  Bluefruit.setTxPower(4);
+  Bluefruit.setName("Bluefruit52");
+
+  // Configure and start the BLE Uart service
+  bleuart.begin();
+
+  // Set up and start advertising
+  startAdv();
+
+  Serial.println(F("Please use Adafruit Bluefruit LE app to connect in Controller mode"));
+  Serial.println(F("Then activate/use the sensors, color picker, game controller, etc!"));
+  Serial.println();  
+}
+
+void startAdv(void)
+{
+  // Advertising packet
+  Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
+  Bluefruit.Advertising.addTxPower();
+  
+  // Include the BLE UART (AKA 'NUS') 128-bit UUID
+  Bluefruit.Advertising.addService(bleuart);
+
+  // Secondary Scan Response packet (optional)
+  // Since there is no room for 'Name' in Advertising packet
+  Bluefruit.ScanResponse.addName();
+
+  /* Start Advertising
+   * - Enable auto advertising if disconnected
+   * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
+   * - Timeout for fast mode is 30 seconds
+   * - Start(timeout) with timeout = 0 will advertise forever (until connected)
+   * 
+   * For recommended advertising interval
+   * https://developer.apple.com/library/content/qa/qa1931/_index.html   
+   */
+  Bluefruit.Advertising.restartOnDisconnect(true);
+  Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
+  Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
+  Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds  
+}
+
+/**************************************************************************/
+/*!
+    @brief  Constantly poll for new command or response data
+*/
+/**************************************************************************/
+void loop(void)
+{
+  // Wait for new data to arrive
+  uint8_t len = readPacket(&bleuart, 500);
+  if (len == 0) return;
+
+  // Got a packet!
+  // printHex(packetbuffer, len);
+
+  // Color
+  if (packetbuffer[1] == 'C') {
+    uint8_t red = packetbuffer[2];
+    uint8_t green = packetbuffer[3];
+    uint8_t blue = packetbuffer[4];
+    Serial.print ("RGB #");
+    if (red < 0x10) Serial.print("0");
+    Serial.print(red, HEX);
+    if (green < 0x10) Serial.print("0");
+    Serial.print(green, HEX);
+    if (blue < 0x10) Serial.print("0");
+    Serial.println(blue, HEX);
+  }
+
+  // Buttons
+  if (packetbuffer[1] == 'B') {
+    uint8_t buttnum = packetbuffer[2] - '0';
+    boolean pressed = packetbuffer[3] - '0';
+    Serial.print ("Button "); Serial.print(buttnum);
+    if (pressed) {
+      Serial.println(" pressed");
+    } else {
+      Serial.println(" released");
+    }
+  }
+
+  // GPS Location
+  if (packetbuffer[1] == 'L') {
+    float lat, lon, alt;
+    lat = parsefloat(packetbuffer+2);
+    lon = parsefloat(packetbuffer+6);
+    alt = parsefloat(packetbuffer+10);
+    Serial.print("GPS Location\t");
+    Serial.print("Lat: "); Serial.print(lat, 4); // 4 digits of precision!
+    Serial.print('\t');
+    Serial.print("Lon: "); Serial.print(lon, 4); // 4 digits of precision!
+    Serial.print('\t');
+    Serial.print(alt, 4); Serial.println(" meters");
+  }
+
+  // Accelerometer
+  if (packetbuffer[1] == 'A') {
+    float x, y, z;
+    x = parsefloat(packetbuffer+2);
+    y = parsefloat(packetbuffer+6);
+    z = parsefloat(packetbuffer+10);
+    Serial.print("Accel\t");
+    Serial.print(x); Serial.print('\t');
+    Serial.print(y); Serial.print('\t');
+    Serial.print(z); Serial.println();
+  }
+
+  // Magnetometer
+  if (packetbuffer[1] == 'M') {
+    float x, y, z;
+    x = parsefloat(packetbuffer+2);
+    y = parsefloat(packetbuffer+6);
+    z = parsefloat(packetbuffer+10);
+    Serial.print("Mag\t");
+    Serial.print(x); Serial.print('\t');
+    Serial.print(y); Serial.print('\t');
+    Serial.print(z); Serial.println();
+  }
+
+  // Gyroscope
+  if (packetbuffer[1] == 'G') {
+    float x, y, z;
+    x = parsefloat(packetbuffer+2);
+    y = parsefloat(packetbuffer+6);
+    z = parsefloat(packetbuffer+10);
+    Serial.print("Gyro\t");
+    Serial.print(x); Serial.print('\t');
+    Serial.print(y); Serial.print('\t');
+    Serial.print(z); Serial.println();
+  }
+
+  // Quaternions
+  if (packetbuffer[1] == 'Q') {
+    float x, y, z, w;
+    x = parsefloat(packetbuffer+2);
+    y = parsefloat(packetbuffer+6);
+    z = parsefloat(packetbuffer+10);
+    w = parsefloat(packetbuffer+14);
+    Serial.print("Quat\t");
+    Serial.print(x); Serial.print('\t');
+    Serial.print(y); Serial.print('\t');
+    Serial.print(z); Serial.print('\t');
+    Serial.print(w); Serial.println();
+  }
+}
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;
+}
+