Revision 4:dedfd6797c10, committed 2019-09-25

Comitter:

avk01

Date:

Wed Sep 25 15:05:11 2019 +0000

Parent:

3:24e365bd1b97

Commit message:

100 Hz with BLE

Changed in this revision

--- a/BLE_API.lib	Thu Nov 05 06:58:30 2015 +0000
+++ b/BLE_API.lib	Wed Sep 25 15:05:11 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#a097e1be76f4
+http://mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#65474dc93927

--- a/main.cpp	Thu Nov 05 06:58:30 2015 +0000
+++ b/main.cpp	Wed Sep 25 15:05:11 2019 +0000
@@ -7,11 +7,13 @@
 #include "nrf51_bitfields.h"
 
 #include "BLE.h"
-#include "DFUService.h"
 #include "UARTService.h"
 
-
-#define LOG(...)    { pc.printf(__VA_ARGS__); }
+//#define CALIBRATE
+#define SERIAL_DEBUG
+#ifdef SERIAL_DEBUG
+#define LOG(...)    { pc.printf(__VA_ARGS__); pc.printf("\r\n"); }
+#endif
 
 #define LED_GREEN   p21
 #define LED_RED     p22
@@ -28,25 +30,28 @@
 #define UART_RTS    p10
 
 /* Starting sampling rate. */
-#define DEFAULT_MPU_HZ  (100)
+#define DEFAULT_MPU_HZ     (100)
+#define DEFAULT_GYRO_FST   (2000) // 250, 500, 1000, 2000
+#define DEFAULT_ACCEL_FST  (8)    // 2, 4, 8, 16
+
 
 DigitalOut blue(LED_BLUE);
 DigitalOut green(LED_GREEN);
 DigitalOut red(LED_RED);
 
 InterruptIn button(BUTTON_PIN);
-AnalogIn    battery(BATTERY_PIN);
+
+#ifdef SERIAL_DEBUG
 Serial pc(UART_TX, UART_RX);
+#endif
 
 InterruptIn motion_probe(p14);
 
 int read_none_count = 0;
-
-BLEDevice  ble;
-UARTService *uartServicePtr;
-
+static const char *devName = "SKQP-B01";
+UARTService *uartService = 0;
 volatile bool bleIsConnected = false;
-volatile uint8_t tick_event = 0;
+volatile bool updatesEnabled = false;
 volatile uint8_t motion_event = 0;
 static signed char board_orientation[9] = {
     1, 0, 0,
@@ -54,37 +59,121 @@
     0, 0, 1
 };
 
+void check_i2c_bus(void);
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
 
-void check_i2c_bus(void);
-unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
+void LedSwitchON(DigitalOut & led)
+{
+    if( led != 0 )
+        led = 0;
+}
+
+void LedSwitchOFF(DigitalOut & led)
+{
+    if( led != 1 )
+        led = 1;
+}
 
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
 {
-    LOG("Connected!\n");
+#ifdef SERIAL_DEBUG
+    LOG("Connected!");
+#endif
+    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+    Gap::ConnectionParams_t connectionParams;
+    ble.gap().getPreferredConnectionParams(&connectionParams);
+    connectionParams.minConnectionInterval =  6;
+    connectionParams.maxConnectionInterval = 10;
+    if (ble.gap().updateConnectionParams(params->handle, &connectionParams) == BLE_ERROR_NONE) {
+#ifdef SERIAL_DEBUG
+        LOG("ConnectionParams Update Accepted!");
+#endif
+    }
     bleIsConnected = true;
+    blue  = 0;
 }
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
 {
-    LOG("Disconnected!\n");
-    LOG("Restarting the advertising process\n");
-    ble.startAdvertising();
+#ifdef SERIAL_DEBUG
+    LOG("Disconnected: %d!",cbParams->reason);
+    LOG("Restarting the advertising process");
+#endif
+    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+    ble.gap().startAdvertising();
     bleIsConnected = false;
+    updatesEnabled = false;
+    blue  = 1;
+}
+
+void  onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
+{
+    if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
+        updatesEnabled = true;
+//        LOG("onUpdatesEnabledCallback");
+    }
+}
+
+void  onUpdatesDisabledCallback(GattAttribute::Handle_t handle)
+{
+    if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
+        updatesEnabled = false;
+//        LOG("onUpdatesDisabledCallback");
+    }
+}
+
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE &ble          = params->ble;
+    ble_error_t error = params->error;
+    if (error != BLE_ERROR_NONE) {
+        return;
+    }
+    uint8_t pass[6] = {'1', '2', '3', '4', '5', '6'};
+    ble.securityManager().init(true,true,SecurityManager::IO_CAPS_DISPLAY_ONLY,pass);
+    ble.gap().onDisconnection(disconnectionCallback);
+    ble.gap().onConnection(connectionCallback);
+    ble.gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
+    ble.gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
+    //
+    uartService = new UARTService(ble);
+    /* setup advertising */
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.gap().setDeviceName((const uint8_t*)devName);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
+    ble.gap().setAdvertisingInterval(8); /* 5ms; in multiples of 0.625ms. */
+    ble.gap().startAdvertising();
+#ifdef SERIAL_DEBUG
+//    LOG("bleInitComplete %s!",ble.getVersion());
+#endif
 }
 
 void tick(void)
 {
-    static uint32_t count = 0;
-    
-    LOG("%d\r\n", count++);
+    if( bleIsConnected ) {
+        LedSwitchOFF(green);
+        if( updatesEnabled ) {
+            LedSwitchOFF(red);
+            blue = !blue;
+            return;
+
+        }
+        LedSwitchOFF(blue);
+        red = !red;
+        return;
+    }
+    LedSwitchOFF(blue);
+    LedSwitchOFF(red);
     green = !green;
 }
 
-void detect(void)
+void ButtonReset(void)
 {
-    LOG("Button pressed\n");  
-    blue = !blue;
+    NVIC_SystemReset();
 }
 
 void motion_interrupt_handle(void)
@@ -92,160 +181,108 @@
     motion_event = 1;
 }
 
-void tap_cb(unsigned char direction, unsigned char count)
-{
-    LOG("Tap motion detected\n");
-}
-
-void android_orient_cb(unsigned char orientation)
-{
-    LOG("Oriention changed\n");
-}
-
-
 int main(void)
 {
     blue  = 1;
     green = 1;
     red   = 1;
-
+#ifdef SERIAL_DEBUG
     pc.baud(115200);
-    
     wait(1);
-    
-    LOG("---- Seeed Tiny BLE ----\n");
-    
+    LOG("---- Seeed Tiny BLE ----");
+#endif
     mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
     mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
-    
-
     if (mpu_init(0)) {
-        LOG("failed to initialize mpu6050\r\n");
+#ifdef SERIAL_DEBUG
+        LOG("failed to initialize mpu6050");
+#endif
     }
-    
     /* Get/set hardware configuration. Start gyro. */
     /* Wake up all sensors. */
     mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
     /* Push both gyro and accel data into the FIFO. */
     mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
-    mpu_set_sample_rate(DEFAULT_MPU_HZ);
-    
+    mpu_set_sample_rate(4*DEFAULT_MPU_HZ);
+    mpu_set_gyro_fsr(DEFAULT_GYRO_FST);
+    mpu_set_accel_fsr(DEFAULT_ACCEL_FST);
     /* Read back configuration in case it was set improperly. */
-    unsigned char accel_fsr;
+    /* unsigned char accel_fsr;
     unsigned short gyro_rate, gyro_fsr;
     mpu_get_sample_rate(&gyro_rate);
     mpu_get_gyro_fsr(&gyro_fsr);
-    mpu_get_accel_fsr(&accel_fsr);
-    
+    mpu_get_accel_fsr(&accel_fsr); */
     dmp_load_motion_driver_firmware();
-    dmp_set_orientation(
-        inv_orientation_matrix_to_scalar(board_orientation));
-    dmp_register_tap_cb(tap_cb);
-    dmp_register_android_orient_cb(android_orient_cb);
-    
-    uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
-                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
-                       DMP_FEATURE_GYRO_CAL;
-    dmp_enable_feature(dmp_features);
+    dmp_set_orientation(inv_orientation_matrix_to_scalar(board_orientation));
+    dmp_enable_feature(DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_LP_QUAT);
     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
     mpu_set_dmp_state(1);
-    
-    dmp_set_interrupt_mode(DMP_INT_GESTURE);
-    dmp_set_tap_thresh(TAP_XYZ, 50);
-    
-    
+    dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
     motion_probe.fall(motion_interrupt_handle);
-
-
-    
+    button.fall(ButtonReset);
+    //
+    unsigned long sensor_timestamp = 0;
+    short gyro[3], accel[3], sensors;
+    long quat[4];
+#ifdef CALIBRATE
+    LedSwitchON(red);
+    while (sensor_timestamp < 20000) {
+        unsigned char more = 1;
+        dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
+    }
+    LedSwitchOFF(red);
+#endif
     Ticker ticker;
-    ticker.attach(tick, 3);
-
-    button.fall(detect);
-
-    LOG("Initialising the nRF51822\n");
-    ble.init();
-    ble.gap().onDisconnection(disconnectionCallback);
-    ble.gap().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.gap().startAdvertising();
-    
+    ticker.attach(tick, 1);
+//
+#ifdef SERIAL_DEBUG
+    LOG("Initialising the nRF51822");
+#endif
+    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+    ble.init(bleInitComplete);
+    while(ble.hasInitialized() == false);     // wait for init
+    //
+    unsigned char dataBuffer[20];
+    memset(dataBuffer,0,sizeof(dataBuffer));
+    dataBuffer[16] = 'E';
+    dataBuffer[17] = 'N';
+    dataBuffer[18] = 'D';
+    dataBuffer[19] =  0;
     while (true) {
-        if (motion_event) {
-            
-            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);
-                    }
+        ble.waitForEvent();
+        if ( !motion_event || !updatesEnabled )
+            continue;
+        unsigned char more = 1;
+        while (more) {
+            ble.waitForEvent();
+            if( dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more) == 0 ) {
+                if ((sensors & INV_XYZ_GYRO) && (sensors & INV_XYZ_ACCEL)) {
+                    memcpy(dataBuffer,&sensor_timestamp,4);
+                    memcpy(dataBuffer+4,accel,6);
+                    memcpy(dataBuffer+10,gyro,6);
+                    uartService->write(dataBuffer,20);
+#ifdef SERIAL_DEBUG
+//                    LOG("%d: %d, %d, %d, %d, %d, %d", sensor_timestamp, accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
+#endif
                 }
             }
-            
-            motion_event = 0;
-        } else {
-            ble.waitForEvent();
+            if (sensors) {
+                read_none_count = 0;
+                continue;
+            }
+            read_none_count++;
+            if (read_none_count > 2) {
+                read_none_count = 0;
+#ifdef SERIAL_DEBUG
+                LOG("I2C may be stuck @ %d", sensor_timestamp);
+#endif
+                ble.waitForEvent();
+                mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
+                ble.waitForEvent();
+                break;
+            }
         }
+        motion_event = 0;
     }
 }
 
@@ -277,8 +314,6 @@
 unsigned short inv_orientation_matrix_to_scalar(
     const signed char *mtx)
 {
-    unsigned short scalar;
-
     /*
        XYZ  010_001_000 Identity Matrix
        XZY  001_010_000
@@ -287,12 +322,9 @@
        ZXY  001_000_010
        ZYX  000_001_010
      */
-
-    scalar = inv_row_2_scale(mtx);
+    unsigned short scalar = inv_row_2_scale(mtx);
     scalar |= inv_row_2_scale(mtx + 3) << 3;
     scalar |= inv_row_2_scale(mtx + 6) << 6;
-
-
     return scalar;
 }
 

--- a/mbed.bld	Thu Nov 05 06:58:30 2015 +0000
+++ b/mbed.bld	Wed Sep 25 15:05:11 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9296ab0bfc11
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- a/nRF51822.lib	Thu Nov 05 06:58:30 2015 +0000
+++ b/nRF51822.lib	Wed Sep 25 15:05:11 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/Nordic-Semiconductor/code/nRF51822/#bf85bf7e73d5
+http://mbed.org/teams/Nordic-Semiconductor/code/nRF51822/#c90ae1400bf2