Justin B / Mbed 2 deprecated TinyBLE_motion_sensorBufferTemplate

Pull request for i.a. sensor buffer template

Dependencies:   BLE_API MPU6050 mbed nRF51822

main.cpp@0:7c12d0922052, 2018-01-23 (annotated)

Committer:
pietermaljaars
Date:
Tue Jan 23 09:04:55 2018 +0000
Revision:
0:7c12d0922052
Child:
1:b529e733ab71
Initial creation of project

Who changed what in which revision?

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