Alexey Krasnoperov / Mbed 2 deprecated SKQP

Test of 100 Hz

Dependencies:   mbed BLE_API nRF51822 eMPL_MPU6050

main.cpp@4:dedfd6797c10, 2019-09-25 (annotated)

Committer:
avk01
Date:
Wed Sep 25 15:05:11 2019 +0000
Revision:
4:dedfd6797c10
Parent:
3:24e365bd1b97 
100 Hz with BLE

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 "UARTService.h"
yihui 0:26da608265f8 11
avk01 4:dedfd6797c10 12 //#define CALIBRATE
avk01 4:dedfd6797c10 13 #define SERIAL_DEBUG
avk01 4:dedfd6797c10 14 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 15 #define LOG(...) { pc.printf(__VA_ARGS__); pc.printf("\r\n"); }
avk01 4:dedfd6797c10 16 #endif
yihui 0:26da608265f8 17
yihui 0:26da608265f8 18 #define LED_GREEN p21
yihui 0:26da608265f8 19 #define LED_RED p22
yihui 0:26da608265f8 20 #define LED_BLUE p23
yihui 0:26da608265f8 21 #define BUTTON_PIN p17
yihui 0:26da608265f8 22 #define BATTERY_PIN p1
yihui 0:26da608265f8 23
yihui 0:26da608265f8 24 #define MPU6050_SDA p12
yihui 0:26da608265f8 25 #define MPU6050_SCL p13
yihui 0:26da608265f8 26
yihui 0:26da608265f8 27 #define UART_TX p9
yihui 0:26da608265f8 28 #define UART_RX p11
yihui 0:26da608265f8 29 #define UART_CTS p8
yihui 0:26da608265f8 30 #define UART_RTS p10
yihui 0:26da608265f8 31
yihui 0:26da608265f8 32 /* Starting sampling rate. */
avk01 4:dedfd6797c10 33 #define DEFAULT_MPU_HZ (100)
avk01 4:dedfd6797c10 34 #define DEFAULT_GYRO_FST (2000) // 250, 500, 1000, 2000
avk01 4:dedfd6797c10 35 #define DEFAULT_ACCEL_FST (8) // 2, 4, 8, 16
avk01 4:dedfd6797c10 36
yihui 0:26da608265f8 37
yihui 0:26da608265f8 38 DigitalOut blue(LED_BLUE);
yihui 0:26da608265f8 39 DigitalOut green(LED_GREEN);
yihui 0:26da608265f8 40 DigitalOut red(LED_RED);
yihui 0:26da608265f8 41
yihui 0:26da608265f8 42 InterruptIn button(BUTTON_PIN);
avk01 4:dedfd6797c10 43
avk01 4:dedfd6797c10 44 #ifdef SERIAL_DEBUG
yihui 0:26da608265f8 45 Serial pc(UART_TX, UART_RX);
avk01 4:dedfd6797c10 46 #endif
yihui 0:26da608265f8 47
yihui 0:26da608265f8 48 InterruptIn motion_probe(p14);
yihui 0:26da608265f8 49
yihui 2:b61ddbb8528e 50 int read_none_count = 0;
avk01 4:dedfd6797c10 51 static const char *devName = "SKQP-B01";
avk01 4:dedfd6797c10 52 UARTService *uartService = 0;
yihui 0:26da608265f8 53 volatile bool bleIsConnected = false;
avk01 4:dedfd6797c10 54 volatile bool updatesEnabled = false;
yihui 0:26da608265f8 55 volatile uint8_t motion_event = 0;
yihui 0:26da608265f8 56 static signed char board_orientation[9] = {
yihui 0:26da608265f8 57 1, 0, 0,
yihui 0:26da608265f8 58 0, 1, 0,
yihui 0:26da608265f8 59 0, 0, 1
yihui 0:26da608265f8 60 };
yihui 0:26da608265f8 61
avk01 4:dedfd6797c10 62 void check_i2c_bus(void);
avk01 4:dedfd6797c10 63 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
yihui 0:26da608265f8 64
avk01 4:dedfd6797c10 65 void LedSwitchON(DigitalOut & led)
avk01 4:dedfd6797c10 66 {
avk01 4:dedfd6797c10 67 if( led != 0 )
avk01 4:dedfd6797c10 68 led = 0;
avk01 4:dedfd6797c10 69 }
avk01 4:dedfd6797c10 70
avk01 4:dedfd6797c10 71 void LedSwitchOFF(DigitalOut & led)
avk01 4:dedfd6797c10 72 {
avk01 4:dedfd6797c10 73 if( led != 1 )
avk01 4:dedfd6797c10 74 led = 1;
avk01 4:dedfd6797c10 75 }
yihui 0:26da608265f8 76
yihui 2:b61ddbb8528e 77
yihui 3:24e365bd1b97 78 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
yihui 0:26da608265f8 79 {
avk01 4:dedfd6797c10 80 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 81 LOG("Connected!");
avk01 4:dedfd6797c10 82 #endif
avk01 4:dedfd6797c10 83 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
avk01 4:dedfd6797c10 84 Gap::ConnectionParams_t connectionParams;
avk01 4:dedfd6797c10 85 ble.gap().getPreferredConnectionParams(&connectionParams);
avk01 4:dedfd6797c10 86 connectionParams.minConnectionInterval = 6;
avk01 4:dedfd6797c10 87 connectionParams.maxConnectionInterval = 10;
avk01 4:dedfd6797c10 88 if (ble.gap().updateConnectionParams(params->handle, &connectionParams) == BLE_ERROR_NONE) {
avk01 4:dedfd6797c10 89 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 90 LOG("ConnectionParams Update Accepted!");
avk01 4:dedfd6797c10 91 #endif
avk01 4:dedfd6797c10 92 }
yihui 0:26da608265f8 93 bleIsConnected = true;
avk01 4:dedfd6797c10 94 blue = 0;
yihui 0:26da608265f8 95 }
yihui 0:26da608265f8 96
yihui 3:24e365bd1b97 97 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
yihui 0:26da608265f8 98 {
avk01 4:dedfd6797c10 99 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 100 LOG("Disconnected: %d!",cbParams->reason);
avk01 4:dedfd6797c10 101 LOG("Restarting the advertising process");
avk01 4:dedfd6797c10 102 #endif
avk01 4:dedfd6797c10 103 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
avk01 4:dedfd6797c10 104 ble.gap().startAdvertising();
yihui 0:26da608265f8 105 bleIsConnected = false;
avk01 4:dedfd6797c10 106 updatesEnabled = false;
avk01 4:dedfd6797c10 107 blue = 1;
avk01 4:dedfd6797c10 108 }
avk01 4:dedfd6797c10 109
avk01 4:dedfd6797c10 110 void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
avk01 4:dedfd6797c10 111 {
avk01 4:dedfd6797c10 112 if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
avk01 4:dedfd6797c10 113 updatesEnabled = true;
avk01 4:dedfd6797c10 114 // LOG("onUpdatesEnabledCallback");
avk01 4:dedfd6797c10 115 }
avk01 4:dedfd6797c10 116 }
avk01 4:dedfd6797c10 117
avk01 4:dedfd6797c10 118 void onUpdatesDisabledCallback(GattAttribute::Handle_t handle)
avk01 4:dedfd6797c10 119 {
avk01 4:dedfd6797c10 120 if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
avk01 4:dedfd6797c10 121 updatesEnabled = false;
avk01 4:dedfd6797c10 122 // LOG("onUpdatesDisabledCallback");
avk01 4:dedfd6797c10 123 }
avk01 4:dedfd6797c10 124 }
avk01 4:dedfd6797c10 125
avk01 4:dedfd6797c10 126 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
avk01 4:dedfd6797c10 127 {
avk01 4:dedfd6797c10 128 BLE &ble = params->ble;
avk01 4:dedfd6797c10 129 ble_error_t error = params->error;
avk01 4:dedfd6797c10 130 if (error != BLE_ERROR_NONE) {
avk01 4:dedfd6797c10 131 return;
avk01 4:dedfd6797c10 132 }
avk01 4:dedfd6797c10 133 uint8_t pass[6] = {'1', '2', '3', '4', '5', '6'};
avk01 4:dedfd6797c10 134 ble.securityManager().init(true,true,SecurityManager::IO_CAPS_DISPLAY_ONLY,pass);
avk01 4:dedfd6797c10 135 ble.gap().onDisconnection(disconnectionCallback);
avk01 4:dedfd6797c10 136 ble.gap().onConnection(connectionCallback);
avk01 4:dedfd6797c10 137 ble.gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
avk01 4:dedfd6797c10 138 ble.gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
avk01 4:dedfd6797c10 139 //
avk01 4:dedfd6797c10 140 uartService = new UARTService(ble);
avk01 4:dedfd6797c10 141 /* setup advertising */
avk01 4:dedfd6797c10 142 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
avk01 4:dedfd6797c10 143 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
avk01 4:dedfd6797c10 144 ble.gap().setDeviceName((const uint8_t*)devName);
avk01 4:dedfd6797c10 145 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
avk01 4:dedfd6797c10 146 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
avk01 4:dedfd6797c10 147 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
avk01 4:dedfd6797c10 148 ble.gap().setAdvertisingInterval(8); /* 5ms; in multiples of 0.625ms. */
avk01 4:dedfd6797c10 149 ble.gap().startAdvertising();
avk01 4:dedfd6797c10 150 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 151 // LOG("bleInitComplete %s!",ble.getVersion());
avk01 4:dedfd6797c10 152 #endif
yihui 0:26da608265f8 153 }
yihui 0:26da608265f8 154
yihui 0:26da608265f8 155 void tick(void)
yihui 0:26da608265f8 156 {
avk01 4:dedfd6797c10 157 if( bleIsConnected ) {
avk01 4:dedfd6797c10 158 LedSwitchOFF(green);
avk01 4:dedfd6797c10 159 if( updatesEnabled ) {
avk01 4:dedfd6797c10 160 LedSwitchOFF(red);
avk01 4:dedfd6797c10 161 blue = !blue;
avk01 4:dedfd6797c10 162 return;
avk01 4:dedfd6797c10 163
avk01 4:dedfd6797c10 164 }
avk01 4:dedfd6797c10 165 LedSwitchOFF(blue);
avk01 4:dedfd6797c10 166 red = !red;
avk01 4:dedfd6797c10 167 return;
avk01 4:dedfd6797c10 168 }
avk01 4:dedfd6797c10 169 LedSwitchOFF(blue);
avk01 4:dedfd6797c10 170 LedSwitchOFF(red);
yihui 0:26da608265f8 171 green = !green;
yihui 0:26da608265f8 172 }
yihui 0:26da608265f8 173
avk01 4:dedfd6797c10 174 void ButtonReset(void)
yihui 0:26da608265f8 175 {
avk01 4:dedfd6797c10 176 NVIC_SystemReset();
yihui 0:26da608265f8 177 }
yihui 0:26da608265f8 178
yihui 0:26da608265f8 179 void motion_interrupt_handle(void)
yihui 0:26da608265f8 180 {
yihui 0:26da608265f8 181 motion_event = 1;
yihui 0:26da608265f8 182 }
yihui 0:26da608265f8 183
yihui 0:26da608265f8 184 int main(void)
yihui 0:26da608265f8 185 {
yihui 0:26da608265f8 186 blue = 1;
yihui 0:26da608265f8 187 green = 1;
yihui 0:26da608265f8 188 red = 1;
avk01 4:dedfd6797c10 189 #ifdef SERIAL_DEBUG
yihui 0:26da608265f8 190 pc.baud(115200);
yihui 2:b61ddbb8528e 191 wait(1);
avk01 4:dedfd6797c10 192 LOG("---- Seeed Tiny BLE ----");
avk01 4:dedfd6797c10 193 #endif
yihui 2:b61ddbb8528e 194 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 195 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 196 if (mpu_init(0)) {
avk01 4:dedfd6797c10 197 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 198 LOG("failed to initialize mpu6050");
avk01 4:dedfd6797c10 199 #endif
yihui 0:26da608265f8 200 }
yihui 0:26da608265f8 201 /* Get/set hardware configuration. Start gyro. */
yihui 0:26da608265f8 202 /* Wake up all sensors. */
yihui 0:26da608265f8 203 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 204 /* Push both gyro and accel data into the FIFO. */
yihui 0:26da608265f8 205 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
avk01 4:dedfd6797c10 206 mpu_set_sample_rate(4*DEFAULT_MPU_HZ);
avk01 4:dedfd6797c10 207 mpu_set_gyro_fsr(DEFAULT_GYRO_FST);
avk01 4:dedfd6797c10 208 mpu_set_accel_fsr(DEFAULT_ACCEL_FST);
yihui 0:26da608265f8 209 /* Read back configuration in case it was set improperly. */
avk01 4:dedfd6797c10 210 /* unsigned char accel_fsr;
yihui 0:26da608265f8 211 unsigned short gyro_rate, gyro_fsr;
yihui 0:26da608265f8 212 mpu_get_sample_rate(&gyro_rate);
yihui 0:26da608265f8 213 mpu_get_gyro_fsr(&gyro_fsr);
avk01 4:dedfd6797c10 214 mpu_get_accel_fsr(&accel_fsr); */
yihui 0:26da608265f8 215 dmp_load_motion_driver_firmware();
avk01 4:dedfd6797c10 216 dmp_set_orientation(inv_orientation_matrix_to_scalar(board_orientation));
avk01 4:dedfd6797c10 217 dmp_enable_feature(DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_LP_QUAT);
yihui 0:26da608265f8 218 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 219 mpu_set_dmp_state(1);
avk01 4:dedfd6797c10 220 dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
yihui 2:b61ddbb8528e 221 motion_probe.fall(motion_interrupt_handle);
avk01 4:dedfd6797c10 222 button.fall(ButtonReset);
avk01 4:dedfd6797c10 223 //
avk01 4:dedfd6797c10 224 unsigned long sensor_timestamp = 0;
avk01 4:dedfd6797c10 225 short gyro[3], accel[3], sensors;
avk01 4:dedfd6797c10 226 long quat[4];
avk01 4:dedfd6797c10 227 #ifdef CALIBRATE
avk01 4:dedfd6797c10 228 LedSwitchON(red);
avk01 4:dedfd6797c10 229 while (sensor_timestamp < 20000) {
avk01 4:dedfd6797c10 230 unsigned char more = 1;
avk01 4:dedfd6797c10 231 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
avk01 4:dedfd6797c10 232 }
avk01 4:dedfd6797c10 233 LedSwitchOFF(red);
avk01 4:dedfd6797c10 234 #endif
yihui 2:b61ddbb8528e 235 Ticker ticker;
avk01 4:dedfd6797c10 236 ticker.attach(tick, 1);
avk01 4:dedfd6797c10 237 //
avk01 4:dedfd6797c10 238 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 239 LOG("Initialising the nRF51822");
avk01 4:dedfd6797c10 240 #endif
avk01 4:dedfd6797c10 241 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
avk01 4:dedfd6797c10 242 ble.init(bleInitComplete);
avk01 4:dedfd6797c10 243 while(ble.hasInitialized() == false); // wait for init
avk01 4:dedfd6797c10 244 //
avk01 4:dedfd6797c10 245 unsigned char dataBuffer[20];
avk01 4:dedfd6797c10 246 memset(dataBuffer,0,sizeof(dataBuffer));
avk01 4:dedfd6797c10 247 dataBuffer[16] = 'E';
avk01 4:dedfd6797c10 248 dataBuffer[17] = 'N';
avk01 4:dedfd6797c10 249 dataBuffer[18] = 'D';
avk01 4:dedfd6797c10 250 dataBuffer[19] = 0;
yihui 0:26da608265f8 251 while (true) {
avk01 4:dedfd6797c10 252 ble.waitForEvent();
avk01 4:dedfd6797c10 253 if ( !motion_event || !updatesEnabled )
avk01 4:dedfd6797c10 254 continue;
avk01 4:dedfd6797c10 255 unsigned char more = 1;
avk01 4:dedfd6797c10 256 while (more) {
avk01 4:dedfd6797c10 257 ble.waitForEvent();
avk01 4:dedfd6797c10 258 if( dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more) == 0 ) {
avk01 4:dedfd6797c10 259 if ((sensors & INV_XYZ_GYRO) && (sensors & INV_XYZ_ACCEL)) {
avk01 4:dedfd6797c10 260 memcpy(dataBuffer,&sensor_timestamp,4);
avk01 4:dedfd6797c10 261 memcpy(dataBuffer+4,accel,6);
avk01 4:dedfd6797c10 262 memcpy(dataBuffer+10,gyro,6);
avk01 4:dedfd6797c10 263 uartService->write(dataBuffer,20);
avk01 4:dedfd6797c10 264 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 265 // LOG("%d: %d, %d, %d, %d, %d, %d", sensor_timestamp, accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
avk01 4:dedfd6797c10 266 #endif
yihui 3:24e365bd1b97 267 }
yihui 3:24e365bd1b97 268 }
avk01 4:dedfd6797c10 269 if (sensors) {
avk01 4:dedfd6797c10 270 read_none_count = 0;
avk01 4:dedfd6797c10 271 continue;
avk01 4:dedfd6797c10 272 }
avk01 4:dedfd6797c10 273 read_none_count++;
avk01 4:dedfd6797c10 274 if (read_none_count > 2) {
avk01 4:dedfd6797c10 275 read_none_count = 0;
avk01 4:dedfd6797c10 276 #ifdef SERIAL_DEBUG
avk01 4:dedfd6797c10 277 LOG("I2C may be stuck @ %d", sensor_timestamp);
avk01 4:dedfd6797c10 278 #endif
avk01 4:dedfd6797c10 279 ble.waitForEvent();
avk01 4:dedfd6797c10 280 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
avk01 4:dedfd6797c10 281 ble.waitForEvent();
avk01 4:dedfd6797c10 282 break;
avk01 4:dedfd6797c10 283 }
yihui 0:26da608265f8 284 }
avk01 4:dedfd6797c10 285 motion_event = 0;
yihui 0:26da608265f8 286 }
yihui 0:26da608265f8 287 }
yihui 0:26da608265f8 288
yihui 0:26da608265f8 289 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 290 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 291 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 292 */
yihui 0:26da608265f8 293 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 294 {
yihui 0:26da608265f8 295 unsigned short b;
yihui 0:26da608265f8 296
yihui 0:26da608265f8 297 if (row[0] > 0)
yihui 0:26da608265f8 298 b = 0;
yihui 0:26da608265f8 299 else if (row[0] < 0)
yihui 0:26da608265f8 300 b = 4;
yihui 0:26da608265f8 301 else if (row[1] > 0)
yihui 0:26da608265f8 302 b = 1;
yihui 0:26da608265f8 303 else if (row[1] < 0)
yihui 0:26da608265f8 304 b = 5;
yihui 0:26da608265f8 305 else if (row[2] > 0)
yihui 0:26da608265f8 306 b = 2;
yihui 0:26da608265f8 307 else if (row[2] < 0)
yihui 0:26da608265f8 308 b = 6;
yihui 0:26da608265f8 309 else
yihui 0:26da608265f8 310 b = 7; // error
yihui 0:26da608265f8 311 return b;
yihui 0:26da608265f8 312 }
yihui 0:26da608265f8 313
yihui 0:26da608265f8 314 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 315 const signed char *mtx)
yihui 0:26da608265f8 316 {
yihui 0:26da608265f8 317 /*
yihui 0:26da608265f8 318 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 319 XZY 001_010_000
yihui 0:26da608265f8 320 YXZ 010_000_001
yihui 0:26da608265f8 321 YZX 000_010_001
yihui 0:26da608265f8 322 ZXY 001_000_010
yihui 0:26da608265f8 323 ZYX 000_001_010
yihui 0:26da608265f8 324 */
avk01 4:dedfd6797c10 325 unsigned short scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 326 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 327 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 328 return scalar;
yihui 0:26da608265f8 329 }
yihui 0:26da608265f8 330