Diff: main.cpp

Revision:

3:24e365bd1b97

Parent:

2:b61ddbb8528e

Child:

4:1704ddee411f

--- a/main.cpp	Thu Nov 05 02:46:37 2015 +0000
+++ b/main.cpp	Thu Nov 05 06:58:30 2015 +0000
@@ -6,7 +6,7 @@
 #include "nrf51.h"
 #include "nrf51_bitfields.h"
 
-#include "BLEDevice.h"
+#include "BLE.h"
 #include "DFUService.h"
 #include "UARTService.h"
 
@@ -28,7 +28,7 @@
 #define UART_RTS    p10
 
 /* Starting sampling rate. */
-#define DEFAULT_MPU_HZ  (200)
+#define DEFAULT_MPU_HZ  (100)
 
 DigitalOut blue(LED_BLUE);
 DigitalOut green(LED_GREEN);
@@ -59,13 +59,13 @@
 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
 
 
-void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
+void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
 {
     LOG("Connected!\n");
     bleIsConnected = true;
 }
 
-void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
 {
     LOG("Disconnected!\n");
     LOG("Restarting the advertising process\n");
@@ -75,6 +75,9 @@
 
 void tick(void)
 {
+    static uint32_t count = 0;
+    
+    LOG("%d\r\n", count++);
     green = !green;
 }
 
@@ -87,61 +90,6 @@
 void motion_interrupt_handle(void)
 {
     motion_event = 1;
-    
-    {
-        unsigned long sensor_timestamp;
-        short gyro[3], accel[3], sensors;
-        long quat[4];
-        unsigned char more = 1;
-        
-        while (more) {
-            /* This function gets new data from the FIFO when the DMP is in
-             * use. The FIFO can contain any combination of gyro, accel,
-             * quaternion, and gesture data. The sensors parameter tells the
-             * caller which data fields were actually populated with new data.
-             * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
-             * the FIFO isn't being filled with accel data.
-             * The driver parses the gesture data to determine if a gesture
-             * event has occurred; on an event, the application will be notified
-             * via a callback (assuming that a callback function was properly
-             * registered). The more parameter is non-zero if there are
-             * leftover packets in the FIFO.
-             */
-            dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
-                          &more);
-            /* Gyro and accel data are written to the FIFO by the DMP in chip
-             * frame and hardware units. This behavior is convenient because it
-             * keeps the gyro and accel outputs of dmp_read_fifo and
-             * mpu_read_fifo consistent.
-             */
-            if (sensors & INV_XYZ_GYRO) {
-                // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
-            }
-            if (sensors & INV_XYZ_ACCEL) {
-                LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
-            }
-            
-            /* Unlike gyro and accel, quaternions are written to the FIFO in
-             * the body frame, q30. The orientation is set by the scalar passed
-             * to dmp_set_orientation during initialization.
-             */
-            if (sensors & INV_WXYZ_QUAT) {
-                // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
-            }
-            
-            if (sensors) {
-                read_none_count = 0;
-            } else {
-                read_none_count++;
-                if (read_none_count > 3) {
-                    read_none_count = 0;
-                    
-                    LOG("I2C may be stuck\r\n");
-                    mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
-                }
-            }
-        }
-    }
 }
 
 void tap_cb(unsigned char direction, unsigned char count)
@@ -202,12 +150,13 @@
     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
     mpu_set_dmp_state(1);
     
-//     dmp_set_interrupt_mode(DMP_INT_GESTURE);
+    dmp_set_interrupt_mode(DMP_INT_GESTURE);
     dmp_set_tap_thresh(TAP_XYZ, 50);
     
     
     motion_probe.fall(motion_interrupt_handle);
 
+
     
     Ticker ticker;
     ticker.attach(tick, 3);
@@ -216,8 +165,8 @@
 
     LOG("Initialising the nRF51822\n");
     ble.init();
-    ble.onDisconnection(disconnectionCallback);
-    ble.onConnection(connectionCallback);
+    ble.gap().onDisconnection(disconnectionCallback);
+    ble.gap().onConnection(connectionCallback);
 
 
     /* setup advertising */
@@ -233,13 +182,67 @@
     //uartService.retargetStdout();
 
     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
-    ble.startAdvertising();
+    ble.gap().startAdvertising();
     
     while (true) {
         if (motion_event) {
-            motion_event = 0;
+            
+            unsigned long sensor_timestamp;
+            short gyro[3], accel[3], sensors;
+            long quat[4];
+            unsigned char more = 1;
             
-           
+            while (more) {
+                /* This function gets new data from the FIFO when the DMP is in
+                 * use. The FIFO can contain any combination of gyro, accel,
+                 * quaternion, and gesture data. The sensors parameter tells the
+                 * caller which data fields were actually populated with new data.
+                 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
+                 * the FIFO isn't being filled with accel data.
+                 * The driver parses the gesture data to determine if a gesture
+                 * event has occurred; on an event, the application will be notified
+                 * via a callback (assuming that a callback function was properly
+                 * registered). The more parameter is non-zero if there are
+                 * leftover packets in the FIFO.
+                 */
+                dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
+                              &more);
+                
+                
+                /* Gyro and accel data are written to the FIFO by the DMP in chip
+                 * frame and hardware units. This behavior is convenient because it
+                 * keeps the gyro and accel outputs of dmp_read_fifo and
+                 * mpu_read_fifo consistent.
+                 */
+                if (sensors & INV_XYZ_GYRO) {
+                    // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
+                }
+                if (sensors & INV_XYZ_ACCEL) {
+                    //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
+                }
+                
+                /* Unlike gyro and accel, quaternions are written to the FIFO in
+                 * the body frame, q30. The orientation is set by the scalar passed
+                 * to dmp_set_orientation during initialization.
+                 */
+                if (sensors & INV_WXYZ_QUAT) {
+                    // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
+                }
+                
+                if (sensors) {
+                    read_none_count = 0;
+                } else {
+                    read_none_count++;
+                    if (read_none_count > 3) {
+                        read_none_count = 0;
+                        
+                        LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
+                        mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
+                    }
+                }
+            }
+            
+            motion_event = 0;
         } else {
             ble.waitForEvent();
         }