Pull request for i.a. sensor buffer template

Dependencies:   BLE_API MPU6050 mbed nRF51822

Committer:
pietermaljaars
Date:
Fri Sep 14 19:47:25 2018 +0000
Revision:
1:b529e733ab71
Parent:
0:7c12d0922052
Child:
2:bf1a19d489eb
Add BLE MAC address to broadcast name

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 1:b529e733ab71 170
pietermaljaars 1:b529e733ab71 171 uint8_t name[] = "iot aabbccddeeff";
pietermaljaars 1:b529e733ab71 172
pietermaljaars 1:b529e733ab71 173 Gap::AddressType_t addr_type;
pietermaljaars 1:b529e733ab71 174 Gap::Address_t address;
pietermaljaars 1:b529e733ab71 175 ble_error_t error = ble.gap().getAddress(&addr_type, address);
pietermaljaars 1:b529e733ab71 176 if (error == BLE_ERROR_NONE) {
pietermaljaars 1:b529e733ab71 177 for (int i = 5; i >= 0; i--){
pietermaljaars 1:b529e733ab71 178 //sprintf(name[4 + ((5-i)*2)], "%02x", (uint8_t)address[i]);
pietermaljaars 1:b529e733ab71 179 //sprintf(buffer, "%02x", address[i]);
pietermaljaars 1:b529e733ab71 180 char buffer[3];
pietermaljaars 1:b529e733ab71 181 sprintf(buffer, "%02x", address[i]);
pietermaljaars 1:b529e733ab71 182 name[4 + ((5-i)*2)] = buffer[0];
pietermaljaars 1:b529e733ab71 183 name[4 + ((5-i)*2) + 1] = buffer[1];
pietermaljaars 1:b529e733ab71 184 }
pietermaljaars 1:b529e733ab71 185 }
pietermaljaars 1:b529e733ab71 186 LOG("name = %s\n", name);
pietermaljaars 1:b529e733ab71 187
pietermaljaars 0:7c12d0922052 188 /* setup advertising */
pietermaljaars 0:7c12d0922052 189 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
pietermaljaars 0:7c12d0922052 190 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
pietermaljaars 0:7c12d0922052 191 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
pietermaljaars 1:b529e733ab71 192 name, sizeof(name));
pietermaljaars 0:7c12d0922052 193 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
pietermaljaars 0:7c12d0922052 194 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
pietermaljaars 0:7c12d0922052 195 DFUService dfu(ble);
pietermaljaars 0:7c12d0922052 196 UARTService uartService(ble);
pietermaljaars 0:7c12d0922052 197 uartServicePtr = &uartService;
pietermaljaars 0:7c12d0922052 198 //uartService.retargetStdout();
pietermaljaars 0:7c12d0922052 199
pietermaljaars 0:7c12d0922052 200 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
pietermaljaars 0:7c12d0922052 201 ble.gap().startAdvertising();
pietermaljaars 0:7c12d0922052 202
pietermaljaars 0:7c12d0922052 203 while (true) {
pietermaljaars 0:7c12d0922052 204 if (motion_event) {
pietermaljaars 0:7c12d0922052 205
pietermaljaars 0:7c12d0922052 206 unsigned long sensor_timestamp;
pietermaljaars 0:7c12d0922052 207 short gyro[3], accel[3], sensors;
pietermaljaars 0:7c12d0922052 208 long quat[4];
pietermaljaars 0:7c12d0922052 209 unsigned char more = 1;
pietermaljaars 0:7c12d0922052 210
pietermaljaars 0:7c12d0922052 211 while (more) {
pietermaljaars 0:7c12d0922052 212 /* This function gets new data from the FIFO when the DMP is in
pietermaljaars 0:7c12d0922052 213 * use. The FIFO can contain any combination of gyro, accel,
pietermaljaars 0:7c12d0922052 214 * quaternion, and gesture data. The sensors parameter tells the
pietermaljaars 0:7c12d0922052 215 * caller which data fields were actually populated with new data.
pietermaljaars 0:7c12d0922052 216 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
pietermaljaars 0:7c12d0922052 217 * the FIFO isn't being filled with accel data.
pietermaljaars 0:7c12d0922052 218 * The driver parses the gesture data to determine if a gesture
pietermaljaars 0:7c12d0922052 219 * event has occurred; on an event, the application will be notified
pietermaljaars 0:7c12d0922052 220 * via a callback (assuming that a callback function was properly
pietermaljaars 0:7c12d0922052 221 * registered). The more parameter is non-zero if there are
pietermaljaars 0:7c12d0922052 222 * leftover packets in the FIFO.
pietermaljaars 0:7c12d0922052 223 */
pietermaljaars 0:7c12d0922052 224 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
pietermaljaars 0:7c12d0922052 225 &more);
pietermaljaars 0:7c12d0922052 226
pietermaljaars 0:7c12d0922052 227
pietermaljaars 0:7c12d0922052 228 /* Gyro and accel data are written to the FIFO by the DMP in chip
pietermaljaars 0:7c12d0922052 229 * frame and hardware units. This behavior is convenient because it
pietermaljaars 0:7c12d0922052 230 * keeps the gyro and accel outputs of dmp_read_fifo and
pietermaljaars 0:7c12d0922052 231 * mpu_read_fifo consistent.
pietermaljaars 0:7c12d0922052 232 */
pietermaljaars 0:7c12d0922052 233 if (sensors & INV_XYZ_GYRO) {
pietermaljaars 0:7c12d0922052 234 // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
pietermaljaars 0:7c12d0922052 235 }
pietermaljaars 0:7c12d0922052 236 if (sensors & INV_XYZ_ACCEL) {
pietermaljaars 0:7c12d0922052 237 //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
pietermaljaars 0:7c12d0922052 238 }
pietermaljaars 0:7c12d0922052 239
pietermaljaars 0:7c12d0922052 240 /* Unlike gyro and accel, quaternions are written to the FIFO in
pietermaljaars 0:7c12d0922052 241 * the body frame, q30. The orientation is set by the scalar passed
pietermaljaars 0:7c12d0922052 242 * to dmp_set_orientation during initialization.
pietermaljaars 0:7c12d0922052 243 */
pietermaljaars 0:7c12d0922052 244 if (sensors & INV_WXYZ_QUAT) {
pietermaljaars 0:7c12d0922052 245 // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
pietermaljaars 0:7c12d0922052 246 }
pietermaljaars 0:7c12d0922052 247
pietermaljaars 0:7c12d0922052 248 if (sensors) {
pietermaljaars 0:7c12d0922052 249 read_none_count = 0;
pietermaljaars 0:7c12d0922052 250 } else {
pietermaljaars 0:7c12d0922052 251 read_none_count++;
pietermaljaars 0:7c12d0922052 252 if (read_none_count > 3) {
pietermaljaars 0:7c12d0922052 253 read_none_count = 0;
pietermaljaars 0:7c12d0922052 254
pietermaljaars 0:7c12d0922052 255 LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
pietermaljaars 0:7c12d0922052 256 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
pietermaljaars 0:7c12d0922052 257 }
pietermaljaars 0:7c12d0922052 258 }
pietermaljaars 0:7c12d0922052 259 }
pietermaljaars 0:7c12d0922052 260
pietermaljaars 0:7c12d0922052 261 motion_event = 0;
pietermaljaars 0:7c12d0922052 262 } else {
pietermaljaars 0:7c12d0922052 263 ble.waitForEvent();
pietermaljaars 0:7c12d0922052 264 }
pietermaljaars 0:7c12d0922052 265 }
pietermaljaars 0:7c12d0922052 266 }
pietermaljaars 0:7c12d0922052 267
pietermaljaars 0:7c12d0922052 268 /* These next two functions converts the orientation matrix (see
pietermaljaars 0:7c12d0922052 269 * gyro_orientation) to a scalar representation for use by the DMP.
pietermaljaars 0:7c12d0922052 270 * NOTE: These functions are borrowed from Invensense's MPL.
pietermaljaars 0:7c12d0922052 271 */
pietermaljaars 0:7c12d0922052 272 static inline unsigned short inv_row_2_scale(const signed char *row)
pietermaljaars 0:7c12d0922052 273 {
pietermaljaars 0:7c12d0922052 274 unsigned short b;
pietermaljaars 0:7c12d0922052 275
pietermaljaars 0:7c12d0922052 276 if (row[0] > 0)
pietermaljaars 0:7c12d0922052 277 b = 0;
pietermaljaars 0:7c12d0922052 278 else if (row[0] < 0)
pietermaljaars 0:7c12d0922052 279 b = 4;
pietermaljaars 0:7c12d0922052 280 else if (row[1] > 0)
pietermaljaars 0:7c12d0922052 281 b = 1;
pietermaljaars 0:7c12d0922052 282 else if (row[1] < 0)
pietermaljaars 0:7c12d0922052 283 b = 5;
pietermaljaars 0:7c12d0922052 284 else if (row[2] > 0)
pietermaljaars 0:7c12d0922052 285 b = 2;
pietermaljaars 0:7c12d0922052 286 else if (row[2] < 0)
pietermaljaars 0:7c12d0922052 287 b = 6;
pietermaljaars 0:7c12d0922052 288 else
pietermaljaars 0:7c12d0922052 289 b = 7; // error
pietermaljaars 0:7c12d0922052 290 return b;
pietermaljaars 0:7c12d0922052 291 }
pietermaljaars 0:7c12d0922052 292
pietermaljaars 0:7c12d0922052 293 unsigned short inv_orientation_matrix_to_scalar(
pietermaljaars 0:7c12d0922052 294 const signed char *mtx)
pietermaljaars 0:7c12d0922052 295 {
pietermaljaars 0:7c12d0922052 296 unsigned short scalar;
pietermaljaars 0:7c12d0922052 297
pietermaljaars 0:7c12d0922052 298 /*
pietermaljaars 0:7c12d0922052 299 XYZ 010_001_000 Identity Matrix
pietermaljaars 0:7c12d0922052 300 XZY 001_010_000
pietermaljaars 0:7c12d0922052 301 YXZ 010_000_001
pietermaljaars 0:7c12d0922052 302 YZX 000_010_001
pietermaljaars 0:7c12d0922052 303 ZXY 001_000_010
pietermaljaars 0:7c12d0922052 304 ZYX 000_001_010
pietermaljaars 0:7c12d0922052 305 */
pietermaljaars 0:7c12d0922052 306
pietermaljaars 0:7c12d0922052 307 scalar = inv_row_2_scale(mtx);
pietermaljaars 0:7c12d0922052 308 scalar |= inv_row_2_scale(mtx + 3) << 3;
pietermaljaars 0:7c12d0922052 309 scalar |= inv_row_2_scale(mtx + 6) << 6;
pietermaljaars 0:7c12d0922052 310
pietermaljaars 0:7c12d0922052 311
pietermaljaars 0:7c12d0922052 312 return scalar;
pietermaljaars 0:7c12d0922052 313 }
pietermaljaars 0:7c12d0922052 314