basic test to get quaternion over ble and use it to improve balance board skills
Dependencies: BLE_API eMPL_MPU6050 mbed nRF51822
Fork of Seeed_Tiny_BLE_Get_Started by
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(); }