Diff: main.cpp

Revision:

0:26da608265f8

Child:

2:b61ddbb8528e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 07 08:13:51 2015 +0000
@@ -0,0 +1,301 @@
+
+#include "mbed.h"
+#include "mbed_i2c.h"
+#include "inv_mpu.h"
+#include "inv_mpu_dmp_motion_driver.h"
+
+#include "BLEDevice.h"
+#include "DFUService.h"
+#include "UARTService.h"
+
+
+#define LOG(...)    { pc.printf(__VA_ARGS__); }
+
+#define LED_GREEN   p21
+#define LED_RED     p22
+#define LED_BLUE    p23
+#define BUTTON_PIN  p17
+#define BATTERY_PIN p1
+
+#define MPU6050_SDA p12
+#define MPU6050_SCL p13
+
+#define UART_TX     p9
+#define UART_RX     p11
+#define UART_CTS    p8
+#define UART_RTS    p10
+
+/* Starting sampling rate. */
+#define DEFAULT_MPU_HZ  (100)
+
+DigitalOut blue(LED_BLUE);
+DigitalOut green(LED_GREEN);
+DigitalOut red(LED_RED);
+
+InterruptIn button(BUTTON_PIN);
+AnalogIn    battery(BATTERY_PIN);
+Serial pc(UART_TX, UART_RX);
+
+InterruptIn motion_probe(p14);
+
+BLEDevice  ble;
+UARTService *uartServicePtr;
+
+volatile bool bleIsConnected = false;
+volatile uint8_t tick_event = 0;
+volatile uint8_t motion_event = 0;
+static signed char board_orientation[9] = {
+    1, 0, 0,
+    0, 1, 0,
+    0, 0, 1
+};
+
+
+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");
+    bleIsConnected = true;
+}
+
+void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
+{
+    LOG("Disconnected!\n");
+    LOG("Restarting the advertising process\n");
+    ble.startAdvertising();
+    bleIsConnected = false;
+}
+
+void tick(void)
+{
+    green = !green;
+}
+
+void detect(void)
+{
+    LOG("Button pressed\n");
+    
+    blue = !blue;
+}
+
+void motion_interrupt_handle(void)
+{
+    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;
+
+    pc.baud(115200);
+    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_init(MPU6050_SDA, MPU6050_SCL);
+    motion_probe.fall(motion_interrupt_handle);
+    
+
+    if (mpu_init(0)) {
+//         LOG("failed to initialize mpu6050\r\n");
+    }
+    
+    /* 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);
+    
+    /* Read back configuration in case it was set improperly. */
+    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);
+    
+    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_fifo_rate(DEFAULT_MPU_HZ);
+    mpu_set_dmp_state(1);
+    
+//     dmp_set_interrupt_mode(DMP_INT_GESTURE);
+    dmp_set_tap_thresh(TAP_XYZ, 50);
+
+    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.
+ */
+static inline unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;      // error
+    return b;
+}
+
+unsigned short inv_orientation_matrix_to_scalar(
+    const signed char *mtx)
+{
+    unsigned short scalar;
+
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+
+    return scalar;
+}
+