1 files changed, 184 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();
+  }
+}