Valentin Capaldi / Mbed 2 deprecated Accel_and_Gyro_BLE_eMD5

First test with Seeed Tiny BLE streaming realtime data over BLE and USB Serial. This program is based in the "Tiny BLE Getting started" using the eMotion Driver 5.12 pulling the sensor values from the DMP.

Dependencies:   BLE_API eMPL_MPU6050 mbed nRF51822

main.cpp@0:26da608265f8, 2015-02-07 (annotated)

Committer:
yihui
Date:
Sat Feb 07 08:13:51 2015 +0000
Revision:
0:26da608265f8
Child:
2:b61ddbb8528e
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:26da608265f8 1
yihui 0:26da608265f8 2 #include "mbed.h"
yihui 0:26da608265f8 3 #include "mbed_i2c.h"
yihui 0:26da608265f8 4 #include "inv_mpu.h"
yihui 0:26da608265f8 5 #include "inv_mpu_dmp_motion_driver.h"
yihui 0:26da608265f8 6
yihui 0:26da608265f8 7 #include "BLEDevice.h"
yihui 0:26da608265f8 8 #include "DFUService.h"
yihui 0:26da608265f8 9 #include "UARTService.h"
yihui 0:26da608265f8 10
yihui 0:26da608265f8 11
yihui 0:26da608265f8 12 #define LOG(...) { pc.printf(__VA_ARGS__); }
yihui 0:26da608265f8 13
yihui 0:26da608265f8 14 #define LED_GREEN p21
yihui 0:26da608265f8 15 #define LED_RED p22
yihui 0:26da608265f8 16 #define LED_BLUE p23
yihui 0:26da608265f8 17 #define BUTTON_PIN p17
yihui 0:26da608265f8 18 #define BATTERY_PIN p1
yihui 0:26da608265f8 19
yihui 0:26da608265f8 20 #define MPU6050_SDA p12
yihui 0:26da608265f8 21 #define MPU6050_SCL p13
yihui 0:26da608265f8 22
yihui 0:26da608265f8 23 #define UART_TX p9
yihui 0:26da608265f8 24 #define UART_RX p11
yihui 0:26da608265f8 25 #define UART_CTS p8
yihui 0:26da608265f8 26 #define UART_RTS p10
yihui 0:26da608265f8 27
yihui 0:26da608265f8 28 /* Starting sampling rate. */
yihui 0:26da608265f8 29 #define DEFAULT_MPU_HZ (100)
yihui 0:26da608265f8 30
yihui 0:26da608265f8 31 DigitalOut blue(LED_BLUE);
yihui 0:26da608265f8 32 DigitalOut green(LED_GREEN);
yihui 0:26da608265f8 33 DigitalOut red(LED_RED);
yihui 0:26da608265f8 34
yihui 0:26da608265f8 35 InterruptIn button(BUTTON_PIN);
yihui 0:26da608265f8 36 AnalogIn battery(BATTERY_PIN);
yihui 0:26da608265f8 37 Serial pc(UART_TX, UART_RX);
yihui 0:26da608265f8 38
yihui 0:26da608265f8 39 InterruptIn motion_probe(p14);
yihui 0:26da608265f8 40
yihui 0:26da608265f8 41 BLEDevice ble;
yihui 0:26da608265f8 42 UARTService *uartServicePtr;
yihui 0:26da608265f8 43
yihui 0:26da608265f8 44 volatile bool bleIsConnected = false;
yihui 0:26da608265f8 45 volatile uint8_t tick_event = 0;
yihui 0:26da608265f8 46 volatile uint8_t motion_event = 0;
yihui 0:26da608265f8 47 static signed char board_orientation[9] = {
yihui 0:26da608265f8 48 1, 0, 0,
yihui 0:26da608265f8 49 0, 1, 0,
yihui 0:26da608265f8 50 0, 0, 1
yihui 0:26da608265f8 51 };
yihui 0:26da608265f8 52
yihui 0:26da608265f8 53
yihui 0:26da608265f8 54 void check_i2c_bus(void);
yihui 0:26da608265f8 55 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
yihui 0:26da608265f8 56
yihui 0:26da608265f8 57 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
yihui 0:26da608265f8 58 {
yihui 0:26da608265f8 59 LOG("Connected!\n");
yihui 0:26da608265f8 60 bleIsConnected = true;
yihui 0:26da608265f8 61 }
yihui 0:26da608265f8 62
yihui 0:26da608265f8 63 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
yihui 0:26da608265f8 64 {
yihui 0:26da608265f8 65 LOG("Disconnected!\n");
yihui 0:26da608265f8 66 LOG("Restarting the advertising process\n");
yihui 0:26da608265f8 67 ble.startAdvertising();
yihui 0:26da608265f8 68 bleIsConnected = false;
yihui 0:26da608265f8 69 }
yihui 0:26da608265f8 70
yihui 0:26da608265f8 71 void tick(void)
yihui 0:26da608265f8 72 {
yihui 0:26da608265f8 73 green = !green;
yihui 0:26da608265f8 74 }
yihui 0:26da608265f8 75
yihui 0:26da608265f8 76 void detect(void)
yihui 0:26da608265f8 77 {
yihui 0:26da608265f8 78 LOG("Button pressed\n");
yihui 0:26da608265f8 79
yihui 0:26da608265f8 80 blue = !blue;
yihui 0:26da608265f8 81 }
yihui 0:26da608265f8 82
yihui 0:26da608265f8 83 void motion_interrupt_handle(void)
yihui 0:26da608265f8 84 {
yihui 0:26da608265f8 85 motion_event = 1;
yihui 0:26da608265f8 86 }
yihui 0:26da608265f8 87
yihui 0:26da608265f8 88 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:26da608265f8 89 {
yihui 0:26da608265f8 90 LOG("Tap motion detected\n");
yihui 0:26da608265f8 91 }
yihui 0:26da608265f8 92
yihui 0:26da608265f8 93 void android_orient_cb(unsigned char orientation)
yihui 0:26da608265f8 94 {
yihui 0:26da608265f8 95 LOG("Oriention changed\n");
yihui 0:26da608265f8 96 }
yihui 0:26da608265f8 97
yihui 0:26da608265f8 98
yihui 0:26da608265f8 99 int main(void)
yihui 0:26da608265f8 100 {
yihui 0:26da608265f8 101 blue = 1;
yihui 0:26da608265f8 102 green = 1;
yihui 0:26da608265f8 103 red = 1;
yihui 0:26da608265f8 104
yihui 0:26da608265f8 105 pc.baud(115200);
yihui 0:26da608265f8 106 LOG("---- Seeed Tiny BLE ----\n");
yihui 0:26da608265f8 107
yihui 0:26da608265f8 108 Ticker ticker;
yihui 0:26da608265f8 109 ticker.attach(tick, 3);
yihui 0:26da608265f8 110
yihui 0:26da608265f8 111 button.fall(detect);
yihui 0:26da608265f8 112
yihui 0:26da608265f8 113 LOG("Initialising the nRF51822\n");
yihui 0:26da608265f8 114 ble.init();
yihui 0:26da608265f8 115 ble.onDisconnection(disconnectionCallback);
yihui 0:26da608265f8 116 ble.onConnection(connectionCallback);
yihui 0:26da608265f8 117
yihui 0:26da608265f8 118
yihui 0:26da608265f8 119 /* setup advertising */
yihui 0:26da608265f8 120 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
yihui 0:26da608265f8 121 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
yihui 0:26da608265f8 122 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
yihui 0:26da608265f8 123 (const uint8_t *)"smurfs", sizeof("smurfs"));
yihui 0:26da608265f8 124 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
yihui 0:26da608265f8 125 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
yihui 0:26da608265f8 126 DFUService dfu(ble);
yihui 0:26da608265f8 127 UARTService uartService(ble);
yihui 0:26da608265f8 128 uartServicePtr = &uartService;
yihui 0:26da608265f8 129 //uartService.retargetStdout();
yihui 0:26da608265f8 130
yihui 0:26da608265f8 131 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
yihui 0:26da608265f8 132 ble.startAdvertising();
yihui 0:26da608265f8 133
yihui 0:26da608265f8 134 check_i2c_bus();
yihui 0:26da608265f8 135
yihui 0:26da608265f8 136 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 137 motion_probe.fall(motion_interrupt_handle);
yihui 0:26da608265f8 138
yihui 0:26da608265f8 139
yihui 0:26da608265f8 140 if (mpu_init(0)) {
yihui 0:26da608265f8 141 // LOG("failed to initialize mpu6050\r\n");
yihui 0:26da608265f8 142 }
yihui 0:26da608265f8 143
yihui 0:26da608265f8 144 /* Get/set hardware configuration. Start gyro. */
yihui 0:26da608265f8 145 /* Wake up all sensors. */
yihui 0:26da608265f8 146 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 147 /* Push both gyro and accel data into the FIFO. */
yihui 0:26da608265f8 148 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 149 mpu_set_sample_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 150
yihui 0:26da608265f8 151 /* Read back configuration in case it was set improperly. */
yihui 0:26da608265f8 152 unsigned char accel_fsr;
yihui 0:26da608265f8 153 unsigned short gyro_rate, gyro_fsr;
yihui 0:26da608265f8 154 mpu_get_sample_rate(&gyro_rate);
yihui 0:26da608265f8 155 mpu_get_gyro_fsr(&gyro_fsr);
yihui 0:26da608265f8 156 mpu_get_accel_fsr(&accel_fsr);
yihui 0:26da608265f8 157
yihui 0:26da608265f8 158 dmp_load_motion_driver_firmware();
yihui 0:26da608265f8 159 dmp_set_orientation(
yihui 0:26da608265f8 160 inv_orientation_matrix_to_scalar(board_orientation));
yihui 0:26da608265f8 161 dmp_register_tap_cb(tap_cb);
yihui 0:26da608265f8 162 dmp_register_android_orient_cb(android_orient_cb);
yihui 0:26da608265f8 163
yihui 0:26da608265f8 164 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
yihui 0:26da608265f8 165 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
yihui 0:26da608265f8 166 DMP_FEATURE_GYRO_CAL;
yihui 0:26da608265f8 167 dmp_enable_feature(dmp_features);
yihui 0:26da608265f8 168 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 169 mpu_set_dmp_state(1);
yihui 0:26da608265f8 170
yihui 0:26da608265f8 171 // dmp_set_interrupt_mode(DMP_INT_GESTURE);
yihui 0:26da608265f8 172 dmp_set_tap_thresh(TAP_XYZ, 50);
yihui 0:26da608265f8 173
yihui 0:26da608265f8 174 while (true) {
yihui 0:26da608265f8 175 if (motion_event) {
yihui 0:26da608265f8 176 motion_event = 0;
yihui 0:26da608265f8 177
yihui 0:26da608265f8 178 unsigned long sensor_timestamp;
yihui 0:26da608265f8 179 short gyro[3], accel[3], sensors;
yihui 0:26da608265f8 180 long quat[4];
yihui 0:26da608265f8 181 unsigned char more = 1;
yihui 0:26da608265f8 182
yihui 0:26da608265f8 183 while (more) {
yihui 0:26da608265f8 184 /* This function gets new data from the FIFO when the DMP is in
yihui 0:26da608265f8 185 * use. The FIFO can contain any combination of gyro, accel,
yihui 0:26da608265f8 186 * quaternion, and gesture data. The sensors parameter tells the
yihui 0:26da608265f8 187 * caller which data fields were actually populated with new data.
yihui 0:26da608265f8 188 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 0:26da608265f8 189 * the FIFO isn't being filled with accel data.
yihui 0:26da608265f8 190 * The driver parses the gesture data to determine if a gesture
yihui 0:26da608265f8 191 * event has occurred; on an event, the application will be notified
yihui 0:26da608265f8 192 * via a callback (assuming that a callback function was properly
yihui 0:26da608265f8 193 * registered). The more parameter is non-zero if there are
yihui 0:26da608265f8 194 * leftover packets in the FIFO.
yihui 0:26da608265f8 195 */
yihui 0:26da608265f8 196 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
yihui 0:26da608265f8 197 &more);
yihui 0:26da608265f8 198 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 0:26da608265f8 199 * frame and hardware units. This behavior is convenient because it
yihui 0:26da608265f8 200 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 0:26da608265f8 201 * mpu_read_fifo consistent.
yihui 0:26da608265f8 202 */
yihui 0:26da608265f8 203 if (sensors & INV_XYZ_GYRO) {
yihui 0:26da608265f8 204 // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
yihui 0:26da608265f8 205 }
yihui 0:26da608265f8 206 if (sensors & INV_XYZ_ACCEL) {
yihui 0:26da608265f8 207 // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
yihui 0:26da608265f8 208 }
yihui 0:26da608265f8 209
yihui 0:26da608265f8 210 /* Unlike gyro and accel, quaternions are written to the FIFO in
yihui 0:26da608265f8 211 * the body frame, q30. The orientation is set by the scalar passed
yihui 0:26da608265f8 212 * to dmp_set_orientation during initialization.
yihui 0:26da608265f8 213 */
yihui 0:26da608265f8 214 if (sensors & INV_WXYZ_QUAT) {
yihui 0:26da608265f8 215 // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
yihui 0:26da608265f8 216 }
yihui 0:26da608265f8 217 }
yihui 0:26da608265f8 218 } else {
yihui 0:26da608265f8 219 ble.waitForEvent();
yihui 0:26da608265f8 220 }
yihui 0:26da608265f8 221 }
yihui 0:26da608265f8 222 }
yihui 0:26da608265f8 223
yihui 0:26da608265f8 224 void check_i2c_bus(void)
yihui 0:26da608265f8 225 {
yihui 0:26da608265f8 226
yihui 0:26da608265f8 227 DigitalInOut scl(MPU6050_SCL);
yihui 0:26da608265f8 228 DigitalInOut sda(MPU6050_SDA);
yihui 0:26da608265f8 229
yihui 0:26da608265f8 230 scl.input();
yihui 0:26da608265f8 231 sda.input();
yihui 0:26da608265f8 232 int scl_level = scl;
yihui 0:26da608265f8 233 int sda_level = sda;
yihui 0:26da608265f8 234 if (scl_level == 0 || sda_level == 0) {
yihui 0:26da608265f8 235 printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
yihui 0:26da608265f8 236
yihui 0:26da608265f8 237 scl.output();
yihui 0:26da608265f8 238 for (int i = 0; i < 8; i++) {
yihui 0:26da608265f8 239 scl = 0;
yihui 0:26da608265f8 240 wait_us(10);
yihui 0:26da608265f8 241 scl = 1;
yihui 0:26da608265f8 242 wait_us(10);
yihui 0:26da608265f8 243 }
yihui 0:26da608265f8 244 }
yihui 0:26da608265f8 245
yihui 0:26da608265f8 246 scl.input();
yihui 0:26da608265f8 247
yihui 0:26da608265f8 248 scl_level = scl;
yihui 0:26da608265f8 249 sda_level = sda;
yihui 0:26da608265f8 250 if (scl_level == 0 || sda_level == 0) {
yihui 0:26da608265f8 251 printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
yihui 0:26da608265f8 252 }
yihui 0:26da608265f8 253 }
yihui 0:26da608265f8 254
yihui 0:26da608265f8 255 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 256 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 257 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 258 */
yihui 0:26da608265f8 259 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 260 {
yihui 0:26da608265f8 261 unsigned short b;
yihui 0:26da608265f8 262
yihui 0:26da608265f8 263 if (row[0] > 0)
yihui 0:26da608265f8 264 b = 0;
yihui 0:26da608265f8 265 else if (row[0] < 0)
yihui 0:26da608265f8 266 b = 4;
yihui 0:26da608265f8 267 else if (row[1] > 0)
yihui 0:26da608265f8 268 b = 1;
yihui 0:26da608265f8 269 else if (row[1] < 0)
yihui 0:26da608265f8 270 b = 5;
yihui 0:26da608265f8 271 else if (row[2] > 0)
yihui 0:26da608265f8 272 b = 2;
yihui 0:26da608265f8 273 else if (row[2] < 0)
yihui 0:26da608265f8 274 b = 6;
yihui 0:26da608265f8 275 else
yihui 0:26da608265f8 276 b = 7; // error
yihui 0:26da608265f8 277 return b;
yihui 0:26da608265f8 278 }
yihui 0:26da608265f8 279
yihui 0:26da608265f8 280 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 281 const signed char *mtx)
yihui 0:26da608265f8 282 {
yihui 0:26da608265f8 283 unsigned short scalar;
yihui 0:26da608265f8 284
yihui 0:26da608265f8 285 /*
yihui 0:26da608265f8 286 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 287 XZY 001_010_000
yihui 0:26da608265f8 288 YXZ 010_000_001
yihui 0:26da608265f8 289 YZX 000_010_001
yihui 0:26da608265f8 290 ZXY 001_000_010
yihui 0:26da608265f8 291 ZYX 000_001_010
yihui 0:26da608265f8 292 */
yihui 0:26da608265f8 293
yihui 0:26da608265f8 294 scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 295 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 296 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 297
yihui 0:26da608265f8 298
yihui 0:26da608265f8 299 return scalar;
yihui 0:26da608265f8 300 }
yihui 0:26da608265f8 301