Alexey Krasnoperov / Mbed 2 deprecated SKQP

Test of 100 Hz

Dependencies:   mbed BLE_API nRF51822 eMPL_MPU6050

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