MtM+ / Mbed 2 deprecated MtConnect04S_Motion_Sensing

This example uses MtConnect04S to connect and measure with sensors of accelerometer and pyrometer.

Dependencies:   BLE_API mbed nRF51822

/media/uploads/mtmkimi/mtc04s-mbed-connected.jpeg

3-axis accelerometer and gyroscope with BLE

We have full tutorial, please visit our blog

main.cpp@1:95ebaf1441e7, 2016-09-19 (annotated)

Committer:
bcc6
Date:
Mon Sep 19 10:09:32 2016 +0000
Revision:
1:95ebaf1441e7
Parent:
0:ea043885f792 
update lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mtmkimi 0:ea043885f792 1
mtmkimi 0:ea043885f792 2 #include "mbed.h"
mtmkimi 0:ea043885f792 3 #include "ble/BLE.h"
mtmkimi 0:ea043885f792 4
mtmkimi 0:ea043885f792 5 /* UART printf */
mtmkimi 0:ea043885f792 6 Serial pc(p5, p4);
mtmkimi 0:ea043885f792 7
mtmkimi 0:ea043885f792 8 /* LED blink */
mtmkimi 0:ea043885f792 9 Ticker ledBlinkTicker;
mtmkimi 0:ea043885f792 10 DigitalOut ledR(p16, 1);
mtmkimi 0:ea043885f792 11 DigitalOut ledG(p15, 1);
mtmkimi 0:ea043885f792 12 DigitalOut ledB(p6 , 1);
mtmkimi 0:ea043885f792 13
mtmkimi 0:ea043885f792 14 /* I2C */
mtmkimi 0:ea043885f792 15 #define I2C_BMA250_ADDR (0x30)
mtmkimi 0:ea043885f792 16 #define I2C_BMG160_ADDR (0xd0)
mtmkimi 0:ea043885f792 17 I2C i2c(p14, p13);
mtmkimi 0:ea043885f792 18 char wdata[8];
mtmkimi 0:ea043885f792 19 char rdata[8];
mtmkimi 0:ea043885f792 20
mtmkimi 0:ea043885f792 21 /* Sensor */
mtmkimi 0:ea043885f792 22 Ticker updateSensorTicker;
mtmkimi 0:ea043885f792 23 volatile bool updateSensorTrigger = false;
mtmkimi 0:ea043885f792 24 uint8_t acceRange = 0x0C; // 2G(0x03), 4G(0x05), 8G(0x08), 16G(0x0C)
mtmkimi 0:ea043885f792 25 uint8_t gyroRange = 0x00; // 2000deg/s(0x00), 1000deg/s(0x01), 500deg/s(0x02), 250deg/s(0x03), 125deg/s(0x04)
mtmkimi 0:ea043885f792 26 int16_t acceXYZ[3];
mtmkimi 0:ea043885f792 27 int16_t gyroXYZ[3];
mtmkimi 0:ea043885f792 28 uint8_t accePayload[7];
mtmkimi 0:ea043885f792 29 uint8_t gyroPayload[7];
mtmkimi 0:ea043885f792 30
mtmkimi 0:ea043885f792 31 /* UUID, Device name */
mtmkimi 0:ea043885f792 32 uint16_t sensServUUID = /*0xA000*/0x1811;
mtmkimi 0:ea043885f792 33 uint16_t acceCharUUID = /*0xA001*/0x2A56;
mtmkimi 0:ea043885f792 34 uint16_t gyroCharUUID = /*0xA002*/0x2A57;
mtmkimi 0:ea043885f792 35 static const char DEVICE_NAME[] = "mbed Motion";
mtmkimi 0:ea043885f792 36 static const uint16_t uuid16_list[] = { /*0xA000*/0x1811 };
mtmkimi 0:ea043885f792 37
mtmkimi 0:ea043885f792 38 /* Setup custom characteristics */
mtmkimi 0:ea043885f792 39 GattCharacteristic acceChar( acceCharUUID, accePayload,
mtmkimi 0:ea043885f792 40 sizeof(accePayload), sizeof(accePayload),
mtmkimi 0:ea043885f792 41 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY |
mtmkimi 0:ea043885f792 42 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
mtmkimi 0:ea043885f792 43
mtmkimi 0:ea043885f792 44 GattCharacteristic gyroChar( gyroCharUUID, gyroPayload,
mtmkimi 0:ea043885f792 45 sizeof(gyroPayload), sizeof(gyroPayload),
mtmkimi 0:ea043885f792 46 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY |
mtmkimi 0:ea043885f792 47 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
mtmkimi 0:ea043885f792 48
mtmkimi 0:ea043885f792 49 /* Setup custom service */
mtmkimi 0:ea043885f792 50 GattCharacteristic *characteristics[] = {&acceChar, &gyroChar};
mtmkimi 0:ea043885f792 51 GattService sensServ(sensServUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
mtmkimi 0:ea043885f792 52
mtmkimi 0:ea043885f792 53
mtmkimi 0:ea043885f792 54
mtmkimi 0:ea043885f792 55 void ledBlinkCallback(void)
mtmkimi 0:ea043885f792 56 {
mtmkimi 0:ea043885f792 57 ledR = !ledR;
mtmkimi 0:ea043885f792 58 ledG = !ledG;
mtmkimi 0:ea043885f792 59 ledB = !ledB;
mtmkimi 0:ea043885f792 60 }
mtmkimi 0:ea043885f792 61
mtmkimi 0:ea043885f792 62 void updateSensorCallback(void)
mtmkimi 0:ea043885f792 63 {
mtmkimi 0:ea043885f792 64 updateSensorTrigger = true;
mtmkimi 0:ea043885f792 65 }
mtmkimi 0:ea043885f792 66
mtmkimi 0:ea043885f792 67 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
mtmkimi 0:ea043885f792 68 {
mtmkimi 0:ea043885f792 69 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
mtmkimi 0:ea043885f792 70 }
mtmkimi 0:ea043885f792 71
mtmkimi 0:ea043885f792 72 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
mtmkimi 0:ea043885f792 73 {
mtmkimi 0:ea043885f792 74 BLE &ble = params->ble;
mtmkimi 0:ea043885f792 75 ble_error_t error = params->error;
mtmkimi 0:ea043885f792 76
mtmkimi 0:ea043885f792 77 if (error != BLE_ERROR_NONE) {
mtmkimi 0:ea043885f792 78 return;
mtmkimi 0:ea043885f792 79 }
mtmkimi 0:ea043885f792 80
mtmkimi 0:ea043885f792 81 ble.gap().onDisconnection(disconnectionCallback);
mtmkimi 0:ea043885f792 82
mtmkimi 0:ea043885f792 83 /* Setup primary service. */
mtmkimi 0:ea043885f792 84 ble.addService(sensServ);
mtmkimi 0:ea043885f792 85
mtmkimi 0:ea043885f792 86 /* Setup advertising. */
mtmkimi 0:ea043885f792 87 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mtmkimi 0:ea043885f792 88 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
mtmkimi 0:ea043885f792 89 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_TAG);
mtmkimi 0:ea043885f792 90 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mtmkimi 0:ea043885f792 91 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mtmkimi 0:ea043885f792 92 ble.gap().setAdvertisingInterval(500); /* 500ms */
mtmkimi 0:ea043885f792 93 ble.gap().startAdvertising();
mtmkimi 0:ea043885f792 94 }
mtmkimi 0:ea043885f792 95
mtmkimi 0:ea043885f792 96 void i2cConfigBMA250()
mtmkimi 0:ea043885f792 97 {
mtmkimi 0:ea043885f792 98 int status;
mtmkimi 0:ea043885f792 99
mtmkimi 0:ea043885f792 100 wdata[0] = 0x14; // SOFTRESET_REG
mtmkimi 0:ea043885f792 101 wdata[1] = 0xB6; // Reset value
mtmkimi 0:ea043885f792 102 status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 103 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 104
mtmkimi 0:ea043885f792 105 wdata[0] = 0x0F; // RANGE_REG
mtmkimi 0:ea043885f792 106 wdata[1] = 0x0C; // (+/-)16G
mtmkimi 0:ea043885f792 107 status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 108 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 109
mtmkimi 0:ea043885f792 110 wdata[0] = 0x10; // BANDWIDTH_REG
mtmkimi 0:ea043885f792 111 wdata[1] = 0x0D; // 250Hz
mtmkimi 0:ea043885f792 112 status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 113 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 114
mtmkimi 0:ea043885f792 115 return;
mtmkimi 0:ea043885f792 116 FAIL:
mtmkimi 0:ea043885f792 117 printf("Error !!\n");
mtmkimi 0:ea043885f792 118 while(1);
mtmkimi 0:ea043885f792 119 }
mtmkimi 0:ea043885f792 120
mtmkimi 0:ea043885f792 121 void i2cConfigBMG160()
mtmkimi 0:ea043885f792 122 {
mtmkimi 0:ea043885f792 123 int status;
mtmkimi 0:ea043885f792 124
mtmkimi 0:ea043885f792 125 wdata[0] = 0x10; // BANDWIDTH_REG
mtmkimi 0:ea043885f792 126 wdata[1] = 0x00; // Unfiltered
mtmkimi 0:ea043885f792 127 status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 128 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 129
mtmkimi 0:ea043885f792 130 wdata[0] = 0x0F; // RANGE_REG
mtmkimi 0:ea043885f792 131 wdata[1] = 0x00; // (+/-)2000deg/s
mtmkimi 0:ea043885f792 132 status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 133 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 134
mtmkimi 0:ea043885f792 135 wdata[0] = 0x1A; //
mtmkimi 0:ea043885f792 136 wdata[1] = 0x20; // slow_offset_unfilt = 1
mtmkimi 0:ea043885f792 137 status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 138 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 139
mtmkimi 0:ea043885f792 140 wdata[0] = 0x31; // SOC_REG
mtmkimi 0:ea043885f792 141 wdata[1] = 0x07; // slow_offset_en_x/y/z = 1
mtmkimi 0:ea043885f792 142 status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
mtmkimi 0:ea043885f792 143 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 144
mtmkimi 0:ea043885f792 145 return;
mtmkimi 0:ea043885f792 146 FAIL:
mtmkimi 0:ea043885f792 147 printf("Error !!\n");
mtmkimi 0:ea043885f792 148 while(1);
mtmkimi 0:ea043885f792 149 }
mtmkimi 0:ea043885f792 150
mtmkimi 0:ea043885f792 151 void i2cMeasureBMA250(int16_t &ax, int16_t &ay, int16_t &az)
mtmkimi 0:ea043885f792 152 {
mtmkimi 0:ea043885f792 153 int status;
mtmkimi 0:ea043885f792 154
mtmkimi 0:ea043885f792 155 wdata[0] = 0x02;
mtmkimi 0:ea043885f792 156 status = i2c.write(I2C_BMA250_ADDR, wdata, 1, 0);
mtmkimi 0:ea043885f792 157 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 158 status = i2c.read(I2C_BMA250_ADDR, rdata, 6, 0);
mtmkimi 0:ea043885f792 159 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 160 // printf("aXYZ_raw = 0x%02X%02X,0x%02X%02X,0x%02X%02X\n",
mtmkimi 0:ea043885f792 161 // rdata[1], rdata[0], rdata[3], rdata[2], rdata[5], rdata[4]);
mtmkimi 0:ea043885f792 162
mtmkimi 0:ea043885f792 163 ax = ((int16_t)rdata[1] << 8) | (rdata[0] & 0xC0);
mtmkimi 0:ea043885f792 164 ay = ((int16_t)rdata[3] << 8) | (rdata[2] & 0xC0);
mtmkimi 0:ea043885f792 165 az = ((int16_t)rdata[5] << 8) | (rdata[4] & 0xC0);
mtmkimi 0:ea043885f792 166
mtmkimi 0:ea043885f792 167 /* Align right */
mtmkimi 0:ea043885f792 168 ax >>= 6;
mtmkimi 0:ea043885f792 169 ay >>= 6;
mtmkimi 0:ea043885f792 170 az >>= 6;
mtmkimi 0:ea043885f792 171
mtmkimi 0:ea043885f792 172 return;
mtmkimi 0:ea043885f792 173 FAIL:
mtmkimi 0:ea043885f792 174 printf("Error !!\n");
mtmkimi 0:ea043885f792 175 while(1);
mtmkimi 0:ea043885f792 176 }
mtmkimi 0:ea043885f792 177
mtmkimi 0:ea043885f792 178 void i2cMeasureBMG160(int16_t &gx, int16_t &gy, int16_t &gz)
mtmkimi 0:ea043885f792 179 {
mtmkimi 0:ea043885f792 180 int status;
mtmkimi 0:ea043885f792 181
mtmkimi 0:ea043885f792 182 wdata[0] = 0x02;
mtmkimi 0:ea043885f792 183 status = i2c.write(I2C_BMG160_ADDR, wdata, 1, 0);
mtmkimi 0:ea043885f792 184 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 185 status = i2c.read(I2C_BMG160_ADDR, rdata, 6, 0);
mtmkimi 0:ea043885f792 186 if (status != 0) goto FAIL;
mtmkimi 0:ea043885f792 187 // printf("gXYZ_raw = 0x%02X%02X,0x%02X%02X,0x%02X%02X\n",
mtmkimi 0:ea043885f792 188 // rdata[1], rdata[0], rdata[3], rdata[2], rdata[5], rdata[4]);
mtmkimi 0:ea043885f792 189
mtmkimi 0:ea043885f792 190 gx = ((int16_t)rdata[1] << 8) | rdata[0];
mtmkimi 0:ea043885f792 191 gy = ((int16_t)rdata[3] << 8) | rdata[2];
mtmkimi 0:ea043885f792 192 gz = ((int16_t)rdata[5] << 8) | rdata[4];
mtmkimi 0:ea043885f792 193
mtmkimi 0:ea043885f792 194 return;
mtmkimi 0:ea043885f792 195 FAIL:
mtmkimi 0:ea043885f792 196 printf("Error !!\n");
mtmkimi 0:ea043885f792 197 while(1);
mtmkimi 0:ea043885f792 198 }
mtmkimi 0:ea043885f792 199
mtmkimi 0:ea043885f792 200 int main(void)
mtmkimi 0:ea043885f792 201 {
mtmkimi 0:ea043885f792 202 printf("~ Hell World ~\n");
mtmkimi 0:ea043885f792 203
mtmkimi 0:ea043885f792 204 /* LED blink ticker */
mtmkimi 0:ea043885f792 205 ledBlinkTicker.attach(&ledBlinkCallback, 1);
mtmkimi 0:ea043885f792 206
mtmkimi 0:ea043885f792 207 /* Update Sensor ticker */
mtmkimi 0:ea043885f792 208 updateSensorTicker.attach(&updateSensorCallback, 0.1);
mtmkimi 0:ea043885f792 209
mtmkimi 0:ea043885f792 210 /* Init BLE */
mtmkimi 0:ea043885f792 211 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
mtmkimi 0:ea043885f792 212 ble.init(bleInitComplete);
mtmkimi 0:ea043885f792 213 while (ble.hasInitialized() == false) { /* spin loop */ }
mtmkimi 0:ea043885f792 214
mtmkimi 0:ea043885f792 215 /* Config sensor */
mtmkimi 0:ea043885f792 216 i2cConfigBMA250();
mtmkimi 0:ea043885f792 217 i2cConfigBMG160();
mtmkimi 0:ea043885f792 218
mtmkimi 0:ea043885f792 219 /* Main loop */
mtmkimi 0:ea043885f792 220 while (1) {
mtmkimi 0:ea043885f792 221 if (updateSensorTrigger && ble.getGapState().connected) {
mtmkimi 0:ea043885f792 222 updateSensorTrigger = false;
mtmkimi 0:ea043885f792 223
mtmkimi 0:ea043885f792 224 /* Get sensor data */
mtmkimi 0:ea043885f792 225 i2cMeasureBMA250(acceXYZ[0], acceXYZ[1], acceXYZ[2]);
mtmkimi 0:ea043885f792 226 i2cMeasureBMG160(gyroXYZ[0], gyroXYZ[1], gyroXYZ[2]);
mtmkimi 0:ea043885f792 227 // printf("aXYZ(%6d,%6d,%6d), gXYZ(%6d,%6d,%6d)\n", acceXYZ[0], acceXYZ[1], acceXYZ[2], gyroXYZ[0], gyroXYZ[1], gyroXYZ[2]);
mtmkimi 0:ea043885f792 228
mtmkimi 0:ea043885f792 229 /* Write data to client */
mtmkimi 0:ea043885f792 230 accePayload[0] = acceRange;
mtmkimi 0:ea043885f792 231 accePayload[1] = (uint8_t)(acceXYZ[0] >> 8);
mtmkimi 0:ea043885f792 232 accePayload[2] = (uint8_t)(acceXYZ[0] >> 0);
mtmkimi 0:ea043885f792 233 accePayload[3] = (uint8_t)(acceXYZ[1] >> 8);
mtmkimi 0:ea043885f792 234 accePayload[4] = (uint8_t)(acceXYZ[1] >> 0);
mtmkimi 0:ea043885f792 235 accePayload[5] = (uint8_t)(acceXYZ[2] >> 8);
mtmkimi 0:ea043885f792 236 accePayload[6] = (uint8_t)(acceXYZ[2] >> 0);
mtmkimi 0:ea043885f792 237 ble.gattServer().write(acceChar.getValueHandle(), accePayload, sizeof(accePayload));
mtmkimi 0:ea043885f792 238 gyroPayload[0] = gyroRange;
mtmkimi 0:ea043885f792 239 gyroPayload[1] = (uint8_t)(gyroXYZ[0] >> 8);
mtmkimi 0:ea043885f792 240 gyroPayload[2] = (uint8_t)(gyroXYZ[0] >> 0);
mtmkimi 0:ea043885f792 241 gyroPayload[3] = (uint8_t)(gyroXYZ[1] >> 8);
mtmkimi 0:ea043885f792 242 gyroPayload[4] = (uint8_t)(gyroXYZ[1] >> 0);
mtmkimi 0:ea043885f792 243 gyroPayload[5] = (uint8_t)(gyroXYZ[2] >> 8);
mtmkimi 0:ea043885f792 244 gyroPayload[6] = (uint8_t)(gyroXYZ[2] >> 0);
mtmkimi 0:ea043885f792 245 ble.gattServer().write(gyroChar.getValueHandle(), gyroPayload, sizeof(gyroPayload));
mtmkimi 0:ea043885f792 246 } else {
mtmkimi 0:ea043885f792 247 /* low power wait for event */
mtmkimi 0:ea043885f792 248 ble.waitForEvent();
mtmkimi 0:ea043885f792 249 }
mtmkimi 0:ea043885f792 250 }
mtmkimi 0:ea043885f792 251 }