main.cpp

00001 
00002 #include "mbed.h"
00003 #include "mbed_i2c.h"
00004 #include "inv_mpu.h"
00005 #include "inv_mpu_dmp_motion_driver.h"
00006 #include "nrf51.h"
00007 #include "nrf51_bitfields.h"
00008 
00009 #include "BLE.h"
00010 #include "DFUService.h"
00011 #include "UARTService.h"
00012 
00013 
00014 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00015 
00016 #define LED_GREEN   p21
00017 #define LED_RED     p22
00018 #define LED_BLUE    p23
00019 #define BUTTON_PIN  p17
00020 #define BATTERY_PIN p1
00021 
00022 #define MPU6050_SDA p12
00023 #define MPU6050_SCL p13
00024 
00025 #define UART_TX     p9
00026 #define UART_RX     p11
00027 #define UART_CTS    p8
00028 #define UART_RTS    p10
00029 
00030 /* Starting sampling rate. */
00031 #define DEFAULT_MPU_HZ  (100)
00032 
00033 DigitalOut blue(LED_BLUE);
00034 DigitalOut green(LED_GREEN);
00035 DigitalOut red(LED_RED);
00036 
00037 InterruptIn button(BUTTON_PIN);
00038 AnalogIn    battery(BATTERY_PIN);
00039 Serial pc(UART_TX, UART_RX);
00040 
00041 InterruptIn motion_probe(p14);
00042 
00043 int read_none_count = 0;
00044 
00045 BLEDevice  ble;
00046 UARTService *uartServicePtr;
00047 
00048 volatile bool bleIsConnected = false;
00049 volatile uint8_t tick_event = 0;
00050 volatile uint8_t motion_event = 0;
00051 static signed char board_orientation[9] = {
00052     1, 0, 0,
00053     0, 1, 0,
00054     0, 0, 1
00055 };
00056 
00057 
00058 void check_i2c_bus(void);
00059 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
00060 
00061 
00062 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
00063 {
00064     LOG("Connected!\n");
00065     bleIsConnected = true;
00066 }
00067 
00068 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
00069 {
00070     LOG("Disconnected!\n");
00071     LOG("Restarting the advertising process\n");
00072     ble.startAdvertising();
00073     bleIsConnected = false;
00074 }
00075 
00076 void tick(void)
00077 {
00078     static uint32_t count = 0;
00079     
00080     LOG("%d\r\n", count++);
00081     green = !green;
00082 }
00083 
00084 void detect(void)
00085 {
00086     LOG("Button pressed\n");  
00087     blue = !blue;
00088 }
00089 
00090 void motion_interrupt_handle(void)
00091 {
00092     motion_event = 1;
00093 }
00094 
00095 void tap_cb(unsigned char direction, unsigned char count)
00096 {
00097     LOG("Tap motion detected\n");
00098 }
00099 
00100 void android_orient_cb(unsigned char orientation)
00101 {
00102     LOG("Oriention changed\n");
00103 }
00104 
00105 
00106 int main(void)
00107 {
00108     blue  = 1;
00109     green = 1;
00110     red   = 1;
00111 
00112     pc.baud(115200);
00113     
00114     wait(1);
00115     
00116     LOG("---- Seeed Tiny BLE ----\n");
00117     
00118     mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
00119     mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
00120     
00121 
00122     if (mpu_init(0)) {
00123         LOG("failed to initialize mpu6050\r\n");
00124     }
00125     
00126     /* Get/set hardware configuration. Start gyro. */
00127     /* Wake up all sensors. */
00128     mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00129     /* Push both gyro and accel data into the FIFO. */
00130     mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00131     mpu_set_sample_rate(DEFAULT_MPU_HZ);
00132     
00133     /* Read back configuration in case it was set improperly. */
00134     unsigned char accel_fsr;
00135     unsigned short gyro_rate, gyro_fsr;
00136     mpu_get_sample_rate(&gyro_rate);
00137     mpu_get_gyro_fsr(&gyro_fsr);
00138     mpu_get_accel_fsr(&accel_fsr);
00139     
00140     dmp_load_motion_driver_firmware();
00141     dmp_set_orientation(
00142         inv_orientation_matrix_to_scalar(board_orientation));
00143     dmp_register_tap_cb(tap_cb);
00144     dmp_register_android_orient_cb(android_orient_cb);
00145     
00146     uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
00147                        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
00148                        DMP_FEATURE_GYRO_CAL;
00149     dmp_enable_feature(dmp_features);
00150     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
00151     mpu_set_dmp_state(1);
00152     
00153     dmp_set_interrupt_mode(DMP_INT_GESTURE);
00154     dmp_set_tap_thresh(TAP_XYZ, 50);
00155     
00156     
00157     motion_probe.fall(motion_interrupt_handle);
00158 
00159 
00160     
00161     Ticker ticker;
00162     ticker.attach(tick, 3);
00163 
00164     button.fall(detect);
00165 
00166     LOG("Initialising the nRF51822\n");
00167     ble.init();
00168     ble.gap().onDisconnection(disconnectionCallback);
00169     ble.gap().onConnection(connectionCallback);
00170 
00171 
00172     /* setup advertising */
00173     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00174     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00175     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00176                                      (const uint8_t *)"smurfs", sizeof("smurfs"));
00177     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00178                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00179     DFUService dfu(ble);                                 
00180     UARTService uartService(ble);
00181     uartServicePtr = &uartService;
00182     //uartService.retargetStdout();
00183 
00184     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00185     ble.gap().startAdvertising();
00186     
00187     while (true) {
00188         if (motion_event) {
00189             
00190             unsigned long sensor_timestamp;
00191             short gyro[3], accel[3], sensors;
00192             long quat[4];
00193             unsigned char more = 1;
00194             
00195             while (more) {
00196                 /* This function gets new data from the FIFO when the DMP is in
00197                  * use. The FIFO can contain any combination of gyro, accel,
00198                  * quaternion, and gesture data. The sensors parameter tells the
00199                  * caller which data fields were actually populated with new data.
00200                  * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
00201                  * the FIFO isn't being filled with accel data.
00202                  * The driver parses the gesture data to determine if a gesture
00203                  * event has occurred; on an event, the application will be notified
00204                  * via a callback (assuming that a callback function was properly
00205                  * registered). The more parameter is non-zero if there are
00206                  * leftover packets in the FIFO.
00207                  */
00208                 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
00209                               &more);
00210                 
00211                 
00212                 /* Gyro and accel data are written to the FIFO by the DMP in chip
00213                  * frame and hardware units. This behavior is convenient because it
00214                  * keeps the gyro and accel outputs of dmp_read_fifo and
00215                  * mpu_read_fifo consistent.
00216                  */
00217                 if (sensors & INV_XYZ_GYRO) {
00218                     // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
00219                 }
00220                 if (sensors & INV_XYZ_ACCEL) {
00221                     //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
00222                 }
00223                 
00224                 /* Unlike gyro and accel, quaternions are written to the FIFO in
00225                  * the body frame, q30. The orientation is set by the scalar passed
00226                  * to dmp_set_orientation during initialization.
00227                  */
00228                 if (sensors & INV_WXYZ_QUAT) {
00229                     // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
00230                 }
00231                 
00232                 if (sensors) {
00233                     read_none_count = 0;
00234                 } else {
00235                     read_none_count++;
00236                     if (read_none_count > 3) {
00237                         read_none_count = 0;
00238                         
00239                         LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
00240                         mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
00241                     }
00242                 }
00243             }
00244             
00245             motion_event = 0;
00246         } else {
00247             ble.waitForEvent();
00248         }
00249     }
00250 }
00251 
00252 /* These next two functions converts the orientation matrix (see
00253  * gyro_orientation) to a scalar representation for use by the DMP.
00254  * NOTE: These functions are borrowed from Invensense's MPL.
00255  */
00256 static inline unsigned short inv_row_2_scale(const signed char *row)
00257 {
00258     unsigned short b;
00259 
00260     if (row[0] > 0)
00261         b = 0;
00262     else if (row[0] < 0)
00263         b = 4;
00264     else if (row[1] > 0)
00265         b = 1;
00266     else if (row[1] < 0)
00267         b = 5;
00268     else if (row[2] > 0)
00269         b = 2;
00270     else if (row[2] < 0)
00271         b = 6;
00272     else
00273         b = 7;      // error
00274     return b;
00275 }
00276 
00277 unsigned short inv_orientation_matrix_to_scalar(
00278     const signed char *mtx)
00279 {
00280     unsigned short scalar;
00281 
00282     /*
00283        XYZ  010_001_000 Identity Matrix
00284        XZY  001_010_000
00285        YXZ  010_000_001
00286        YZX  000_010_001
00287        ZXY  001_000_010
00288        ZYX  000_001_010
00289      */
00290 
00291     scalar = inv_row_2_scale(mtx);
00292     scalar |= inv_row_2_scale(mtx + 3) << 3;
00293     scalar |= inv_row_2_scale(mtx + 6) << 6;
00294 
00295 
00296     return scalar;
00297 }
00298