Diff: main.cpp

Revision:

2:02175845b24c

Parent:

1:3eec9883598a

Child:

3:3157e61f2bfd

--- a/main.cpp	Thu Oct 12 18:47:26 2017 +0000
+++ b/main.cpp	Sun Oct 15 18:24:03 2017 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include "MPU9250.h"
+#include "ble/BLE.h"
+#include "ButtonService.h"
 
 // Serial comms
 Serial pc(USBTX, USBRX);
@@ -10,55 +12,177 @@
 // Configuration
 bool testMPUConnection = true;
 bool doSensorInitialization = false;
-bool printAccelerometer = true;
-bool printGyroscope = true;
+bool printAccelerometer = false;
+bool printGyroscope = false;
+
+DigitalOut  led1(LED1);
+InterruptIn button(BUTTON1);
+
+const static char     DEVICE_NAME[] = "Terminator";
+static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
+
+enum {
+    RELEASED = 0,
+    PRESSED,
+    IDLE
+};
+
+static uint8_t buttonState = IDLE;
+
+static ButtonService *buttonServicePtr;
+
+void buttonPressedCallback(void)
+{
+    /* Note that the buttonPressedCallback() executes in interrupt context, so it is safer to access
+     * BLE device API from the main thread. */
+    buttonState = PRESSED;
+}
+
+void buttonReleasedCallback(void)
+{
+    /* Note that the buttonReleasedCallback() executes in interrupt context, so it is safer to access
+     * BLE device API from the main thread. */
+    buttonState = RELEASED;
+}
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+    BLE::Instance().gap().startAdvertising();
+}
+
+void periodicFunctionCallback(void)
+{
+    led1 = !led1; /* Do blinky on LED1 to indicate system aliveness. */
+}
+
+/**
+ * This function is called when the ble initialization process has failled
+ */
+void onBleInitError(BLE &ble, ble_error_t error)
+{
+    /* Initialization error handling should go here */
+}
 
-int main () {
+/**
+ * Callback triggered when the ble initialization process has finished
+ */
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE&        ble   = params->ble;
+    ble_error_t error = params->error;
+
+    if (error != BLE_ERROR_NONE) {
+        /* In case of error, forward the error handling to onBleInitError */
+        onBleInitError(ble, error);
+        return;
+    }
+
+    /* Ensure that it is the default instance of BLE */
+    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
+        return;
+    }
+
+    ble.gap().onDisconnection(disconnectionCallback);
+
+    /* Setup primary service */
+    buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */);
+
+    /* setup advertising */
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
+    ble.gap().startAdvertising();
+
+}
+
+int main ()
+{
+    // Turn the led
+    led1 = 1;
     int16_t accelerometer[3] = {0,0,0};
-    int16_t gyroscope[3] = {0,0,0};    
-        
+    int16_t gyroscope[3] = {0,0,0};
+
+    //Attach a function to be called by the Ticker, specifiying the interval in seconds.
+    Ticker ticker;
+    ticker.attach(periodicFunctionCallback, 1);
+    button.fall(buttonPressedCallback);
+    button.rise(buttonReleasedCallback);
+
+    BLE &ble = BLE::Instance();
+    ble.init(bleInitComplete);
+
+    /* SpinWait for initialization to complete. This is necessary because the
+    * BLE object is used in the main loop below. */
+    while (ble.hasInitialized()  == false) {
+        /* spin loop */
+    }
+
     if (testMPUConnection) {
         uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-        pc.printf("I AM 0x%x\n\r", whoami); 
+        pc.printf("I AM 0x%x\n\r", whoami);
         pc.printf("I SHOULD BE 0x71\n\r");
-        wait(1);   
-    }  
-    
+        wait(1);
+    }
+
     if (doSensorInitialization) {
-        // Initialise sensor board    
+        // Initialise sensor board
         pc.printf("Initializing sensor\n\r");
         mpu.initMPU9250();
         pc.printf("Initialization finished\n\r");
         wait(1);
     }
-        
+
     while(1) {
-        pc.printf("Starting to stream data");                    
-        if(printAccelerometer){
+
+        //pc.printf("Starting to stream data");
+        if(printAccelerometer && printGyroscope) {
             mpu.readAccelData(accelerometer);
             float ax = accelerometer[0] * 2.0 / 32768.0;
             float ay = accelerometer[1] * 2.0 / 32768.0;
             float az = accelerometer[2] * 2.0 / 32768.0;
-            
-            pc.printf("Acelerometer information, AX, AY, AZ \n");
-            pc.printf("(%f, %f, %f)\n", ax,ay,az);
-            
+
+            //pc.printf("Acelerometer information, AX, AY, AZ \n");
+            //pc.printf("(%f, %f, %f)\n", ax,ay,az);
+
             float roll = float(atan2(ay, az) * 180/3.1416f);
             float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
             float yaw = atan(ax/-ay);
-            
-            pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);
-        }
-        
-        if(printGyroscope){
+
+            //pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);
+
             mpu.readGyroData(gyroscope);
             float gx = gyroscope[0] * 250.0 / 32768.0;
             float gy = gyroscope[1] * 250.0 / 32768.0;
             float gz = gyroscope[2] * 250.0 / 32768.0;
-            pc.printf("Gyroscope information, GX, GY, GZ \n");
-            pc.printf("(:%f, %f, %f)\n", gx,gy,gz);
+
+
+            pc.printf("(%f, %f, %f, %f, %f, %f)\n", roll, pitch, yaw, gx, gy, gz);
+
+
         }
-        
-        wait(0.3);
-    } 
+        if (buttonState != IDLE) {
+            //buttonServicePtr->updateButtonState(buttonState);
+            //char myword[] = { 'H', 'e', 'l', 'l', 'o', '\0' };          
+            uint8_t sensorValues[12] = {0};
+            sensorValues[0] = 12;
+            sensorValues[1] = 12;
+            sensorValues[2] = 12;
+            sensorValues[3] = 12;
+            sensorValues[4] = 12;
+            sensorValues[5] = 12;
+            sensorValues[6] = 12;
+            sensorValues[7] = 12;
+            sensorValues[8] = 12;
+            sensorValues[9] = 12;
+            sensorValues[10] = 12;
+            sensorValues[11] = 12;
+            buttonServicePtr->updateButtonState(sensorValues);
+            buttonState = IDLE;
+        }
+        ble.waitForEvent();
+
+        //wait(0.3);
+    }
 }
\ No newline at end of file