Valentin Squirelo / Mbed 2 deprecated balanceboard

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 Seeed

main.cpp@4:1704ddee411f, 2016-08-28 (annotated)

Committer:
squirelo
Date:
Sun Aug 28 16:48:12 2016 +0000
Revision:
4:1704ddee411f
Parent:
3:24e365bd1b97 
basic test with quaternion

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 3:24e365bd1b97 9 #include "BLE.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 3:24e365bd1b97 31 #define DEFAULT_MPU_HZ (100)
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 3:24e365bd1b97 62 void connectionCallback(const Gap::ConnectionCallbackParams_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 3:24e365bd1b97 68 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
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 3:24e365bd1b97 78 static uint32_t count = 0;
yihui 3:24e365bd1b97 79
yihui 3:24e365bd1b97 80 LOG("%d\r\n", count++);
yihui 0:26da608265f8 81 green = !green;
yihui 0:26da608265f8 82 }
yihui 0:26da608265f8 83
yihui 0:26da608265f8 84 void detect(void)
yihui 0:26da608265f8 85 {
yihui 2:b61ddbb8528e 86 LOG("Button pressed\n");
yihui 0:26da608265f8 87 blue = !blue;
yihui 0:26da608265f8 88 }
yihui 0:26da608265f8 89
yihui 0:26da608265f8 90 void motion_interrupt_handle(void)
yihui 0:26da608265f8 91 {
yihui 0:26da608265f8 92 motion_event = 1;
yihui 0:26da608265f8 93 }
yihui 0:26da608265f8 94
yihui 0:26da608265f8 95 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:26da608265f8 96 {
squirelo 4:1704ddee411f 97 LOG("Touched the ground");
yihui 0:26da608265f8 98 }
yihui 0:26da608265f8 99
yihui 0:26da608265f8 100 void android_orient_cb(unsigned char orientation)
yihui 0:26da608265f8 101 {
squirelo 4:1704ddee411f 102 //LOG("Oriention changed\n");
yihui 0:26da608265f8 103 }
yihui 0:26da608265f8 104
yihui 0:26da608265f8 105
yihui 0:26da608265f8 106 int main(void)
yihui 0:26da608265f8 107 {
yihui 0:26da608265f8 108 blue = 1;
yihui 0:26da608265f8 109 green = 1;
yihui 0:26da608265f8 110 red = 1;
yihui 0:26da608265f8 111
yihui 0:26da608265f8 112 pc.baud(115200);
yihui 2:b61ddbb8528e 113
yihui 2:b61ddbb8528e 114 wait(1);
yihui 2:b61ddbb8528e 115
yihui 0:26da608265f8 116 LOG("---- Seeed Tiny BLE ----\n");
yihui 0:26da608265f8 117
yihui 2:b61ddbb8528e 118 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 119 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 120
yihui 0:26da608265f8 121
yihui 0:26da608265f8 122 if (mpu_init(0)) {
yihui 2:b61ddbb8528e 123 LOG("failed to initialize mpu6050\r\n");
yihui 0:26da608265f8 124 }
yihui 0:26da608265f8 125
yihui 0:26da608265f8 126 /* Get/set hardware configuration. Start gyro. */
yihui 0:26da608265f8 127 /* Wake up all sensors. */
yihui 0:26da608265f8 128 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 129 /* Push both gyro and accel data into the FIFO. */
yihui 0:26da608265f8 130 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 131 mpu_set_sample_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 132
yihui 0:26da608265f8 133 /* Read back configuration in case it was set improperly. */
yihui 0:26da608265f8 134 unsigned char accel_fsr;
yihui 0:26da608265f8 135 unsigned short gyro_rate, gyro_fsr;
yihui 0:26da608265f8 136 mpu_get_sample_rate(&gyro_rate);
yihui 0:26da608265f8 137 mpu_get_gyro_fsr(&gyro_fsr);
yihui 0:26da608265f8 138 mpu_get_accel_fsr(&accel_fsr);
yihui 0:26da608265f8 139
yihui 0:26da608265f8 140 dmp_load_motion_driver_firmware();
yihui 0:26da608265f8 141 dmp_set_orientation(
yihui 0:26da608265f8 142 inv_orientation_matrix_to_scalar(board_orientation));
yihui 0:26da608265f8 143 dmp_register_tap_cb(tap_cb);
yihui 0:26da608265f8 144 dmp_register_android_orient_cb(android_orient_cb);
yihui 0:26da608265f8 145
yihui 0:26da608265f8 146 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
yihui 0:26da608265f8 147 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
yihui 0:26da608265f8 148 DMP_FEATURE_GYRO_CAL;
yihui 0:26da608265f8 149 dmp_enable_feature(dmp_features);
yihui 0:26da608265f8 150 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 151 mpu_set_dmp_state(1);
yihui 0:26da608265f8 152
yihui 3:24e365bd1b97 153 dmp_set_interrupt_mode(DMP_INT_GESTURE);
yihui 0:26da608265f8 154 dmp_set_tap_thresh(TAP_XYZ, 50);
yihui 2:b61ddbb8528e 155
yihui 2:b61ddbb8528e 156
yihui 2:b61ddbb8528e 157 motion_probe.fall(motion_interrupt_handle);
yihui 0:26da608265f8 158
yihui 3:24e365bd1b97 159
yihui 2:b61ddbb8528e 160
yihui 2:b61ddbb8528e 161 Ticker ticker;
yihui 2:b61ddbb8528e 162 ticker.attach(tick, 3);
yihui 2:b61ddbb8528e 163
yihui 2:b61ddbb8528e 164 button.fall(detect);
yihui 2:b61ddbb8528e 165
yihui 2:b61ddbb8528e 166 LOG("Initialising the nRF51822\n");
yihui 2:b61ddbb8528e 167 ble.init();
yihui 3:24e365bd1b97 168 ble.gap().onDisconnection(disconnectionCallback);
yihui 3:24e365bd1b97 169 ble.gap().onConnection(connectionCallback);
yihui 2:b61ddbb8528e 170
yihui 2:b61ddbb8528e 171
yihui 2:b61ddbb8528e 172 /* setup advertising */
yihui 2:b61ddbb8528e 173 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
yihui 2:b61ddbb8528e 174 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
yihui 2:b61ddbb8528e 175 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
squirelo 4:1704ddee411f 176 (const uint8_t *)"balanceboard", sizeof("balanceboard"));
yihui 2:b61ddbb8528e 177 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
yihui 2:b61ddbb8528e 178 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
yihui 2:b61ddbb8528e 179 DFUService dfu(ble);
yihui 2:b61ddbb8528e 180 UARTService uartService(ble);
yihui 2:b61ddbb8528e 181 uartServicePtr = &uartService;
yihui 2:b61ddbb8528e 182 //uartService.retargetStdout();
yihui 2:b61ddbb8528e 183
yihui 2:b61ddbb8528e 184 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
yihui 3:24e365bd1b97 185 ble.gap().startAdvertising();
yihui 2:b61ddbb8528e 186
yihui 0:26da608265f8 187 while (true) {
yihui 0:26da608265f8 188 if (motion_event) {
yihui 3:24e365bd1b97 189
yihui 3:24e365bd1b97 190 unsigned long sensor_timestamp;
yihui 3:24e365bd1b97 191 short gyro[3], accel[3], sensors;
yihui 3:24e365bd1b97 192 long quat[4];
yihui 3:24e365bd1b97 193 unsigned char more = 1;
yihui 0:26da608265f8 194
yihui 3:24e365bd1b97 195 while (more) {
yihui 3:24e365bd1b97 196 /* This function gets new data from the FIFO when the DMP is in
yihui 3:24e365bd1b97 197 * use. The FIFO can contain any combination of gyro, accel,
yihui 3:24e365bd1b97 198 * quaternion, and gesture data. The sensors parameter tells the
yihui 3:24e365bd1b97 199 * caller which data fields were actually populated with new data.
yihui 3:24e365bd1b97 200 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 3:24e365bd1b97 201 * the FIFO isn't being filled with accel data.
yihui 3:24e365bd1b97 202 * The driver parses the gesture data to determine if a gesture
yihui 3:24e365bd1b97 203 * event has occurred; on an event, the application will be notified
yihui 3:24e365bd1b97 204 * via a callback (assuming that a callback function was properly
yihui 3:24e365bd1b97 205 * registered). The more parameter is non-zero if there are
yihui 3:24e365bd1b97 206 * leftover packets in the FIFO.
yihui 3:24e365bd1b97 207 */
yihui 3:24e365bd1b97 208 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
yihui 3:24e365bd1b97 209 &more);
yihui 3:24e365bd1b97 210
yihui 3:24e365bd1b97 211
yihui 3:24e365bd1b97 212 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 3:24e365bd1b97 213 * frame and hardware units. This behavior is convenient because it
yihui 3:24e365bd1b97 214 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 3:24e365bd1b97 215 * mpu_read_fifo consistent.
yihui 3:24e365bd1b97 216 */
yihui 3:24e365bd1b97 217 if (sensors & INV_XYZ_GYRO) {
yihui 3:24e365bd1b97 218 // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
yihui 3:24e365bd1b97 219 }
yihui 3:24e365bd1b97 220 if (sensors & INV_XYZ_ACCEL) {
squirelo 4:1704ddee411f 221 // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
yihui 3:24e365bd1b97 222 }
yihui 3:24e365bd1b97 223
yihui 3:24e365bd1b97 224 /* Unlike gyro and accel, quaternions are written to the FIFO in
yihui 3:24e365bd1b97 225 * the body frame, q30. The orientation is set by the scalar passed
yihui 3:24e365bd1b97 226 * to dmp_set_orientation during initialization.
yihui 3:24e365bd1b97 227 */
yihui 3:24e365bd1b97 228 if (sensors & INV_WXYZ_QUAT) {
squirelo 4:1704ddee411f 229 LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
yihui 3:24e365bd1b97 230 }
yihui 3:24e365bd1b97 231
yihui 3:24e365bd1b97 232 if (sensors) {
yihui 3:24e365bd1b97 233 read_none_count = 0;
yihui 3:24e365bd1b97 234 } else {
yihui 3:24e365bd1b97 235 read_none_count++;
yihui 3:24e365bd1b97 236 if (read_none_count > 3) {
yihui 3:24e365bd1b97 237 read_none_count = 0;
yihui 3:24e365bd1b97 238
yihui 3:24e365bd1b97 239 LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
yihui 3:24e365bd1b97 240 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 3:24e365bd1b97 241 }
yihui 3:24e365bd1b97 242 }
yihui 3:24e365bd1b97 243 }
yihui 3:24e365bd1b97 244
yihui 3:24e365bd1b97 245 motion_event = 0;
yihui 0:26da608265f8 246 } else {
yihui 0:26da608265f8 247 ble.waitForEvent();
yihui 0:26da608265f8 248 }
yihui 0:26da608265f8 249 }
yihui 0:26da608265f8 250 }
yihui 0:26da608265f8 251
yihui 0:26da608265f8 252 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 253 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 254 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 255 */
yihui 0:26da608265f8 256 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 257 {
yihui 0:26da608265f8 258 unsigned short b;
yihui 0:26da608265f8 259
yihui 0:26da608265f8 260 if (row[0] > 0)
yihui 0:26da608265f8 261 b = 0;
yihui 0:26da608265f8 262 else if (row[0] < 0)
yihui 0:26da608265f8 263 b = 4;
yihui 0:26da608265f8 264 else if (row[1] > 0)
yihui 0:26da608265f8 265 b = 1;
yihui 0:26da608265f8 266 else if (row[1] < 0)
yihui 0:26da608265f8 267 b = 5;
yihui 0:26da608265f8 268 else if (row[2] > 0)
yihui 0:26da608265f8 269 b = 2;
yihui 0:26da608265f8 270 else if (row[2] < 0)
yihui 0:26da608265f8 271 b = 6;
yihui 0:26da608265f8 272 else
yihui 0:26da608265f8 273 b = 7; // error
yihui 0:26da608265f8 274 return b;
yihui 0:26da608265f8 275 }
yihui 0:26da608265f8 276
yihui 0:26da608265f8 277 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 278 const signed char *mtx)
yihui 0:26da608265f8 279 {
yihui 0:26da608265f8 280 unsigned short scalar;
yihui 0:26da608265f8 281
yihui 0:26da608265f8 282 /*
yihui 0:26da608265f8 283 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 284 XZY 001_010_000
yihui 0:26da608265f8 285 YXZ 010_000_001
yihui 0:26da608265f8 286 YZX 000_010_001
yihui 0:26da608265f8 287 ZXY 001_000_010
yihui 0:26da608265f8 288 ZYX 000_001_010
yihui 0:26da608265f8 289 */
yihui 0:26da608265f8 290
yihui 0:26da608265f8 291 scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 292 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 293 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 294
yihui 0:26da608265f8 295
yihui 0:26da608265f8 296 return scalar;
yihui 0:26da608265f8 297 }
yihui 0:26da608265f8 298