Diff: main.cpp

Revision:

2:b61ddbb8528e

Parent:

0:26da608265f8

Child:

3:24e365bd1b97

--- a/main.cpp	Wed Apr 22 07:47:17 2015 +0000
+++ b/main.cpp	Thu Nov 05 02:46:37 2015 +0000
@@ -3,6 +3,8 @@
 #include "mbed_i2c.h"
 #include "inv_mpu.h"
 #include "inv_mpu_dmp_motion_driver.h"
+#include "nrf51.h"
+#include "nrf51_bitfields.h"
 
 #include "BLEDevice.h"
 #include "DFUService.h"
@@ -26,7 +28,7 @@
 #define UART_RTS    p10
 
 /* Starting sampling rate. */
-#define DEFAULT_MPU_HZ  (100)
+#define DEFAULT_MPU_HZ  (200)
 
 DigitalOut blue(LED_BLUE);
 DigitalOut green(LED_GREEN);
@@ -38,6 +40,8 @@
 
 InterruptIn motion_probe(p14);
 
+int read_none_count = 0;
+
 BLEDevice  ble;
 UARTService *uartServicePtr;
 
@@ -54,6 +58,7 @@
 void check_i2c_bus(void);
 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)
 {
     LOG("Connected!\n");
@@ -75,14 +80,68 @@
 
 void detect(void)
 {
-    LOG("Button pressed\n");
-    
+    LOG("Button pressed\n");  
     blue = !blue;
 }
 
 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)
@@ -103,42 +162,17 @@
     red   = 1;
 
     pc.baud(115200);
+    
+    wait(1);
+    
     LOG("---- Seeed Tiny BLE ----\n");
     
-    Ticker ticker;
-    ticker.attach(tick, 3);
-
-    button.fall(detect);
-
-    LOG("Initialising the nRF51822\n");
-    ble.init();
-    ble.onDisconnection(disconnectionCallback);
-    ble.onConnection(connectionCallback);
-
-
-    /* setup advertising */
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
-    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
-                                     (const uint8_t *)"smurfs", sizeof("smurfs"));
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
-                                     (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
-    DFUService dfu(ble);                                 
-    UARTService uartService(ble);
-    uartServicePtr = &uartService;
-    //uartService.retargetStdout();
-
-    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
-    ble.startAdvertising();
-    
-    check_i2c_bus();
-   
+    mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
     mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
-    motion_probe.fall(motion_interrupt_handle);
     
 
     if (mpu_init(0)) {
-//         LOG("failed to initialize mpu6050\r\n");
+        LOG("failed to initialize mpu6050\r\n");
     }
     
     /* Get/set hardware configuration. Start gyro. */
@@ -170,88 +204,48 @@
     
 //     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);
+
+    button.fall(detect);
+
+    LOG("Initialising the nRF51822\n");
+    ble.init();
+    ble.onDisconnection(disconnectionCallback);
+    ble.onConnection(connectionCallback);
+
+
+    /* setup advertising */
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
+                                     (const uint8_t *)"smurfs", sizeof("smurfs"));
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
+                                     (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
+    DFUService dfu(ble);                                 
+    UARTService uartService(ble);
+    uartServicePtr = &uartService;
+    //uartService.retargetStdout();
+
+    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
+    ble.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]);
-                }
-            }
+           
         } else {
             ble.waitForEvent();
         }
     }
 }
 
-void check_i2c_bus(void)
-{
-    
-    DigitalInOut scl(MPU6050_SCL);
-    DigitalInOut sda(MPU6050_SDA);
-    
-    scl.input();
-    sda.input();
-    int scl_level = scl;
-    int sda_level = sda;
-    if (scl_level == 0 || sda_level == 0) {
-        printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
-        
-        scl.output();
-        for (int i = 0; i < 8; i++) {
-            scl = 0;
-            wait_us(10);
-            scl = 1;
-            wait_us(10);
-        }
-    }
-    
-    scl.input();
-    
-    scl_level = scl;
-    sda_level = sda;
-    if (scl_level == 0 || sda_level == 0) {
-        printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
-    }
-}
-
 /* These next two functions converts the orientation matrix (see
  * gyro_orientation) to a scalar representation for use by the DMP.
  * NOTE: These functions are borrowed from Invensense's MPL.