Alexey Krasnoperov / Mbed 2 deprecated SKQP

Test of 100 Hz

Dependencies:   mbed BLE_API nRF51822 eMPL_MPU6050

main.cpp@2:b61ddbb8528e, 2015-11-05 (annotated)

Committer:
yihui
Date:
Thu Nov 05 02:46:37 2015 +0000
Revision:
2:b61ddbb8528e
Parent:
0:26da608265f8
Child:
3:24e365bd1b97
able to recover from i2c bus fault

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