Nora Vazbyte / Mbed 2 deprecated BLE_Heartrate_VL53L0X

workss

Dependencies:   mbed BLE_API nRF51822 VL53L0X

main.cpp@35:daf134714cee, 2019-03-14 (annotated)

Committer:
vazbyte
Date:
Thu Mar 14 19:42:18 2019 +0000
Revision:
35:daf134714cee
Parent:
34:1d3818f8c1a1
Child:
36:fb7f31e53ed9
created binary logic to encode time + direction of step into 2 8-bit packets

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vazbyte 35:daf134714cee 1 #include <stdio.h>
vazbyte 35:daf134714cee 2 #include <math.h>
mbedAustin 0:cd5b6733aeb1 3 #include "mbed.h"
andresag 19:477567297aac 4 #include "ble/BLE.h"
vazbyte 24:931eeb8a70fc 5 #include "VL53L0X.h"
vazbyte 25:0a0805c118c0 6 #include "ble/services/HeartRateService.h"
vazbyte 31:d1ceadbc6c44 7
vazbyte 29:6ba8491c1dab 8 #define range1_addr (0x56)
vazbyte 29:6ba8491c1dab 9 #define range2_addr (0x60)
vazbyte 29:6ba8491c1dab 10 #define range1_XSHUT p15
vazbyte 29:6ba8491c1dab 11 #define range2_XSHUT p16
vazbyte 29:6ba8491c1dab 12 #define VL53L0_I2C_SDA p30
vazbyte 29:6ba8491c1dab 13 #define VL53L0_I2C_SCL p7
vazbyte 35:daf134714cee 14 #define TIME_SCALE 3 // sensors activated every 100ms * TIME_SCALE = 0.3 seconds
vazbyte 29:6ba8491c1dab 15 #define DIST_MIN 0
vazbyte 29:6ba8491c1dab 16 #define DIST_MAX 22
vazbyte 35:daf134714cee 17 //#define TIMESTAMP_FREQ 3000 // PROD: time granularity is 100ms * TIMESTAMP_FREQ = 5 minutes
vazbyte 35:daf134714cee 18 #define TIMESTAMP_FREQ 300 // DEV: time granularity is 100ms * TIMESTAMP_FREQ = 30 seconds
vazbyte 35:daf134714cee 19 //#define MAX_TIME 1728000 // PROD: keep track of 100ms * MAX_TIME = 2880 minutes = 2 days
vazbyte 35:daf134714cee 20 #define MAX_TIME 172800 // DEV: keep track of 100ms * MAX_TIME = 288 minutes = 4.8 hours
vazbyte 24:931eeb8a70fc 21
vazbyte 24:931eeb8a70fc 22 Serial pc(USBTX, USBRX);
vazbyte 24:931eeb8a70fc 23 static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
vazbyte 24:931eeb8a70fc 24 DigitalOut led1(LED1);
vazbyte 24:931eeb8a70fc 25 DigitalOut led2(LED2);
vazbyte 24:931eeb8a70fc 26 DigitalOut led(LED3, 1);
mbedAustin 9:b33f42191584 27 uint16_t customServiceUUID = 0xA000;
mbedAustin 13:62b1d32745ac 28 uint16_t readCharUUID = 0xA001;
mbedAustin 9:b33f42191584 29 uint16_t writeCharUUID = 0xA002;
vazbyte 31:d1ceadbc6c44 30
vazbyte 24:931eeb8a70fc 31 static DigitalOut shutdown1_pin(range1_XSHUT);
vazbyte 24:931eeb8a70fc 32 static VL53L0X range1(&devI2c, &shutdown1_pin, NC);
vazbyte 24:931eeb8a70fc 33 static DigitalOut shutdown2_pin(range2_XSHUT);
vazbyte 24:931eeb8a70fc 34 static VL53L0X range2(&devI2c, &shutdown2_pin, NC);
vazbyte 31:d1ceadbc6c44 35
vazbyte 33:3fad9e7238a4 36 uint32_t distance1, distance2;
vazbyte 33:3fad9e7238a4 37 int dist1, dist2, status1, status2;
vazbyte 35:daf134714cee 38 int timestamp = 0, current_time = 0;
vazbyte 24:931eeb8a70fc 39
vazbyte 31:d1ceadbc6c44 40 const static int cw = 20 / TIME_SCALE;
vazbyte 35:daf134714cee 41 const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE;
vazbyte 35:daf134714cee 42 const static int maximum_time = MAX_TIME / TIMESTAMP_FREQ;
vazbyte 31:d1ceadbc6c44 43 int countdown = cw;
vazbyte 31:d1ceadbc6c44 44 bool countdown1_triggered = false, countdown2_triggered = false;
vazbyte 31:d1ceadbc6c44 45 bool step_in = false, step_out = false;
vazbyte 34:1d3818f8c1a1 46 bool range1_just_triggered = false, range2_just_triggered = false;
vazbyte 31:d1ceadbc6c44 47
vazbyte 33:3fad9e7238a4 48 const static char DEVICE_NAME[] = "OCCUPY-CRICHTON-ST";
vazbyte 33:3fad9e7238a4 49 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
vazbyte 35:daf134714cee 50 int * encoded_array;
vazbyte 31:d1ceadbc6c44 51
vazbyte 25:0a0805c118c0 52 HeartRateService *hrService;
vazbyte 26:793d65b08afb 53 uint8_t hrmCounter = 0;
vazbyte 31:d1ceadbc6c44 54
vazbyte 32:b9306ebceb61 55 void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
vazbyte 35:daf134714cee 56 printf("Bluetooth connected at %i\n", current_time);
vazbyte 32:b9306ebceb61 57 }
vazbyte 32:b9306ebceb61 58
andresag 19:477567297aac 59 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
mbedAustin 1:94152e7d8b5c 60 {
vazbyte 35:daf134714cee 61 printf("Bluetooth disconnected at %i\n", current_time);
andresag 22:406127954d1f 62 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
mbedAustin 1:94152e7d8b5c 63 }
vazbyte 31:d1ceadbc6c44 64
andresag 22:406127954d1f 65 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
andresag 22:406127954d1f 66 {
andresag 22:406127954d1f 67 BLE &ble = params->ble;
andresag 22:406127954d1f 68 ble_error_t error = params->error;
andresag 22:406127954d1f 69
andresag 22:406127954d1f 70 if (error != BLE_ERROR_NONE) {
andresag 22:406127954d1f 71 return;
andresag 22:406127954d1f 72 }
vazbyte 32:b9306ebceb61 73
vazbyte 32:b9306ebceb61 74 ble.gap().onConnection(connectionCallback);
andresag 19:477567297aac 75 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 25:0a0805c118c0 76 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 31:d1ceadbc6c44 77
vazbyte 33:3fad9e7238a4 78 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 33:3fad9e7238a4 79 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 33:3fad9e7238a4 80 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 33:3fad9e7238a4 81 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
vazbyte 33:3fad9e7238a4 82 ble.gap().setAdvertisingInterval(100);
vazbyte 25:0a0805c118c0 83 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
vazbyte 31:d1ceadbc6c44 84
andresag 19:477567297aac 85 ble.gap().startAdvertising();
andresag 22:406127954d1f 86 }
vazbyte 31:d1ceadbc6c44 87
vazbyte 27:903ec28ea7a0 88 int format_dist(int distance) {
vazbyte 27:903ec28ea7a0 89 int result;
vazbyte 27:903ec28ea7a0 90
vazbyte 27:903ec28ea7a0 91 if (distance > 1270)
vazbyte 27:903ec28ea7a0 92 result = 127;
vazbyte 27:903ec28ea7a0 93 else
vazbyte 27:903ec28ea7a0 94 result = distance/10;
vazbyte 27:903ec28ea7a0 95
vazbyte 27:903ec28ea7a0 96 return result;
vazbyte 27:903ec28ea7a0 97 }
vazbyte 35:daf134714cee 98
vazbyte 35:daf134714cee 99 void dec_to_bin(int decimal, int bin[]) {
vazbyte 35:daf134714cee 100 int temp;
vazbyte 35:daf134714cee 101 // encoding little-endian
vazbyte 35:daf134714cee 102 for (int i = 10; i >= 0; i--) {
vazbyte 35:daf134714cee 103 temp = decimal >> i;
vazbyte 35:daf134714cee 104 bin[i] = temp&1;
vazbyte 35:daf134714cee 105 }
vazbyte 35:daf134714cee 106 }
vazbyte 35:daf134714cee 107
vazbyte 35:daf134714cee 108 int bin_to_dec(int binary) {
vazbyte 35:daf134714cee 109 int rem, temp, dec = 0, b = 1;
vazbyte 35:daf134714cee 110 temp = binary;
vazbyte 35:daf134714cee 111 while (temp > 0)
vazbyte 35:daf134714cee 112 {
vazbyte 35:daf134714cee 113 rem = temp % 10;
vazbyte 35:daf134714cee 114 dec = dec + rem * b;
vazbyte 35:daf134714cee 115 b *= 2;
vazbyte 35:daf134714cee 116 temp /= 10;
vazbyte 35:daf134714cee 117 }
vazbyte 35:daf134714cee 118 // printf("The decimal equivalent of %i is %i", num, dec);
vazbyte 35:daf134714cee 119 return dec;
vazbyte 35:daf134714cee 120 }
vazbyte 35:daf134714cee 121
vazbyte 35:daf134714cee 122 void encode_bin(int direction, int time, int encoding[]) {
vazbyte 35:daf134714cee 123 time = time % maximum_time;
vazbyte 35:daf134714cee 124 int bin[11] = {0};
vazbyte 35:daf134714cee 125 dec_to_bin(time, bin);
vazbyte 35:daf134714cee 126
vazbyte 35:daf134714cee 127 // for checking transmission errors:
vazbyte 35:daf134714cee 128 // sending as 2 messages; designating them with 0 (1st part) and 1`(2nd part)
vazbyte 35:daf134714cee 129 encoding [8] = 1;
vazbyte 35:daf134714cee 130 encoding [0] = 0;
vazbyte 35:daf134714cee 131
vazbyte 35:daf134714cee 132 // used to send the current time when Bluetooth connection established
vazbyte 35:daf134714cee 133 if (direction == -1) {
vazbyte 35:daf134714cee 134 encoding[15] = 1;
vazbyte 35:daf134714cee 135 encoding[7] = 1;
vazbyte 35:daf134714cee 136
vazbyte 35:daf134714cee 137 } else {
vazbyte 35:daf134714cee 138 encoding[15] = 0;
vazbyte 35:daf134714cee 139 encoding[7] = 0;
vazbyte 35:daf134714cee 140
vazbyte 35:daf134714cee 141 // to distinguish whether movement was IN (1) or OUT (0)
vazbyte 35:daf134714cee 142 if (direction == 1) {
vazbyte 35:daf134714cee 143 encoding[1] = 1;
vazbyte 35:daf134714cee 144 } else {
vazbyte 35:daf134714cee 145 encoding[1] = 0;
vazbyte 35:daf134714cee 146 }
vazbyte 35:daf134714cee 147 }
vazbyte 35:daf134714cee 148
vazbyte 35:daf134714cee 149 int count = 10;
vazbyte 35:daf134714cee 150 for (int i = 14; i >= 2; i--) {
vazbyte 35:daf134714cee 151 if (i == 8 || i == 7) { }
vazbyte 35:daf134714cee 152 else {
vazbyte 35:daf134714cee 153 encoding[i] = bin[count];
vazbyte 35:daf134714cee 154 count--;
vazbyte 35:daf134714cee 155 }
vazbyte 35:daf134714cee 156 }
vazbyte 35:daf134714cee 157 }
vazbyte 35:daf134714cee 158
vazbyte 35:daf134714cee 159 void create_packets(int encoding[], double& packet1, double& packet2) {
vazbyte 35:daf134714cee 160 double binary1 = 0, binary2 = 0;
vazbyte 35:daf134714cee 161 for (int i = 0; i < 8; i++) {
vazbyte 35:daf134714cee 162 binary1 += encoding[i] * pow(10, (double)i);
vazbyte 35:daf134714cee 163 binary2 += encoding[8+i] * pow(10, (double)i);
vazbyte 35:daf134714cee 164 }
vazbyte 35:daf134714cee 165 packet1 = bin_to_dec(binary1);
vazbyte 35:daf134714cee 166 packet2 = bin_to_dec(binary2);
vazbyte 35:daf134714cee 167 }
vazbyte 31:d1ceadbc6c44 168
vazbyte 23:52e8e05df60c 169 void wakeup_event_cb() {
vazbyte 35:daf134714cee 170
vazbyte 35:daf134714cee 171 int encoding [16] = {0};
vazbyte 23:52e8e05df60c 172 led != led;
vazbyte 35:daf134714cee 173 timestamp++;
vazbyte 35:daf134714cee 174 ::current_time = timestamp / time_cycle;
vazbyte 24:931eeb8a70fc 175
vazbyte 29:6ba8491c1dab 176 if (countdown1_triggered) {
vazbyte 29:6ba8491c1dab 177 countdown--;
vazbyte 29:6ba8491c1dab 178 if (countdown == 0) {
vazbyte 29:6ba8491c1dab 179 countdown1_triggered = false;
vazbyte 29:6ba8491c1dab 180 }
vazbyte 29:6ba8491c1dab 181 }
vazbyte 29:6ba8491c1dab 182 if (countdown2_triggered) {
vazbyte 29:6ba8491c1dab 183 countdown--;
vazbyte 29:6ba8491c1dab 184 if (countdown == 0) {
vazbyte 29:6ba8491c1dab 185 countdown2_triggered = false;
vazbyte 29:6ba8491c1dab 186 }
vazbyte 29:6ba8491c1dab 187 }
vazbyte 31:d1ceadbc6c44 188
vazbyte 24:931eeb8a70fc 189 status1 = range1.get_distance(&distance1);
vazbyte 24:931eeb8a70fc 190 status2 = range2.get_distance(&distance2);
vazbyte 26:793d65b08afb 191
vazbyte 27:903ec28ea7a0 192 dist1 = format_dist(distance1);
vazbyte 27:903ec28ea7a0 193 dist2 = format_dist(distance2);
vazbyte 31:d1ceadbc6c44 194
vazbyte 24:931eeb8a70fc 195 if (status1 == VL53L0X_ERROR_NONE) {
vazbyte 35:daf134714cee 196 // printf("Range1 [mm]: %6ld\r\n", dist1);
vazbyte 29:6ba8491c1dab 197 if (dist1 > DIST_MIN && dist1 < DIST_MAX) {
vazbyte 24:931eeb8a70fc 198 led1 = 0;
vazbyte 29:6ba8491c1dab 199
vazbyte 29:6ba8491c1dab 200 if (!countdown1_triggered && !countdown2_triggered) {
vazbyte 29:6ba8491c1dab 201 countdown1_triggered = true;
vazbyte 29:6ba8491c1dab 202 countdown = cw;
vazbyte 34:1d3818f8c1a1 203 } else if (countdown2_triggered && !range1_just_triggered) {
vazbyte 29:6ba8491c1dab 204 printf("STEP IN DETECTED\n");
vazbyte 32:b9306ebceb61 205 hrmCounter = 1;
vazbyte 32:b9306ebceb61 206 hrService->updateHeartRate(hrmCounter);
vazbyte 29:6ba8491c1dab 207 countdown2_triggered = false;
vazbyte 29:6ba8491c1dab 208 }
vazbyte 34:1d3818f8c1a1 209
vazbyte 34:1d3818f8c1a1 210 range1_just_triggered = true;
vazbyte 24:931eeb8a70fc 211 }
vazbyte 24:931eeb8a70fc 212 else {
vazbyte 24:931eeb8a70fc 213 led1 = 1;
vazbyte 34:1d3818f8c1a1 214 range1_just_triggered = false;
vazbyte 24:931eeb8a70fc 215 }
vazbyte 24:931eeb8a70fc 216 } else {
vazbyte 34:1d3818f8c1a1 217 // printf("Range1 [mm]: --\r\n");
vazbyte 24:931eeb8a70fc 218 led1 = 1;
vazbyte 34:1d3818f8c1a1 219 range2_just_triggered = false;
vazbyte 24:931eeb8a70fc 220 }
vazbyte 24:931eeb8a70fc 221 if (status2 == VL53L0X_ERROR_NONE) {
vazbyte 35:daf134714cee 222 // printf("Range2 [mm]: %6ld\r\n", dist2);
vazbyte 29:6ba8491c1dab 223 if (dist2 > DIST_MIN && dist2 < DIST_MAX) {
vazbyte 27:903ec28ea7a0 224 led2 = 0;
vazbyte 29:6ba8491c1dab 225
vazbyte 29:6ba8491c1dab 226 if (!countdown1_triggered && !countdown2_triggered) {
vazbyte 29:6ba8491c1dab 227 countdown2_triggered = true;
vazbyte 29:6ba8491c1dab 228 countdown = cw;
vazbyte 34:1d3818f8c1a1 229 } else if (countdown1_triggered && !range2_just_triggered) {
vazbyte 29:6ba8491c1dab 230 printf("STEP OUT DETECTED\n");
vazbyte 32:b9306ebceb61 231 hrmCounter = 16;
vazbyte 32:b9306ebceb61 232 hrService->updateHeartRate(hrmCounter);
vazbyte 29:6ba8491c1dab 233 countdown1_triggered = false;
vazbyte 29:6ba8491c1dab 234 }
vazbyte 34:1d3818f8c1a1 235
vazbyte 34:1d3818f8c1a1 236 range2_just_triggered = true;
vazbyte 34:1d3818f8c1a1 237 }
vazbyte 34:1d3818f8c1a1 238 else {
vazbyte 34:1d3818f8c1a1 239 led2 = 1;
vazbyte 34:1d3818f8c1a1 240 range2_just_triggered = false;
vazbyte 27:903ec28ea7a0 241 }
vazbyte 24:931eeb8a70fc 242 } else {
vazbyte 34:1d3818f8c1a1 243 // printf("Range2 [mm]: --\r\n");
vazbyte 24:931eeb8a70fc 244 led2 = 1;
vazbyte 34:1d3818f8c1a1 245 range2_just_triggered = false;
vazbyte 24:931eeb8a70fc 246 }
vazbyte 23:52e8e05df60c 247 }
vazbyte 31:d1ceadbc6c44 248
andresag 22:406127954d1f 249 int main(void)
andresag 22:406127954d1f 250 {
vazbyte 24:931eeb8a70fc 251 range1.init_sensor(range1_addr);
vazbyte 24:931eeb8a70fc 252 range2.init_sensor(range2_addr);
vazbyte 24:931eeb8a70fc 253
andresag 22:406127954d1f 254 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
andresag 22:406127954d1f 255 ble.init(bleInitComplete);
andresag 22:406127954d1f 256
vazbyte 23:52e8e05df60c 257 Ticker ticker;
vazbyte 31:d1ceadbc6c44 258 ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE);
vazbyte 24:931eeb8a70fc 259
vazbyte 23:52e8e05df60c 260 while (ble.hasInitialized()) {
vazbyte 24:931eeb8a70fc 261 ble.waitForEvent();
mbedAustin 2:e84c13abc479 262 }
andresag 20:fcc752d401ec 263 }