Nora Vazbyte / Mbed OS 99Problems-BLEAint1

app critics will say it's money, cash, toes

source/main.cpp@22:50f88b18cfad, 2018-12-16 (annotated)

Committer:
vazbyte
Date:
Sun Dec 16 13:51:44 2018 +0000
Revision:
22:50f88b18cfad
Parent:
21:36bd27c83b35
Child:
23:7126ef3503e2
added step buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vazbyte 0:07212de2fec1 1 #include <events/mbed_events.h>
vazbyte 3:a33709dad06c 2 #include <math.h>
vazbyte 11:5023e8c93e4d 3
vazbyte 10:8cf4069d064a 4 #include <ctime>
vazbyte 0:07212de2fec1 5 #include <mbed.h>
vazbyte 1:df8884d38960 6 #include "MPU9250.h"
vazbyte 0:07212de2fec1 7 #include "ble/BLE.h"
vazbyte 0:07212de2fec1 8 #include "ble/Gap.h"
vazbyte 1:df8884d38960 9 #include "ble/services/HeartRateService.h"
vazbyte 1:df8884d38960 10
vazbyte 21:36bd27c83b35 11 DigitalOut Led0(p31);
vazbyte 21:36bd27c83b35 12 DigitalOut Led1(p30);
vazbyte 21:36bd27c83b35 13 DigitalOut Led2(p29);
vazbyte 21:36bd27c83b35 14 DigitalOut Led3(p28);
vazbyte 21:36bd27c83b35 15 DigitalOut Led4(p4);
vazbyte 21:36bd27c83b35 16 DigitalOut Led5(p3);
vazbyte 19:1998218bbb63 17 DigitalOut Led6(p11);
vazbyte 19:1998218bbb63 18 DigitalOut Led7(p12);
vazbyte 19:1998218bbb63 19 DigitalOut Led8(p13);
vazbyte 19:1998218bbb63 20 DigitalOut Led9(p14);
vazbyte 19:1998218bbb63 21 DigitalOut Led10(p15);
vazbyte 19:1998218bbb63 22 DigitalOut Led11(p16);
vazbyte 19:1998218bbb63 23 DigitalOut Led12(p17);
vazbyte 19:1998218bbb63 24 DigitalOut Led13(p18);
vazbyte 19:1998218bbb63 25 DigitalOut Led14(p19);
vazbyte 19:1998218bbb63 26 DigitalOut Led15(p20);
vazbyte 19:1998218bbb63 27 DigitalOut Led16(p22);
vazbyte 19:1998218bbb63 28 DigitalOut Led17(p23);
vazbyte 19:1998218bbb63 29 DigitalOut Led18(p24);
vazbyte 19:1998218bbb63 30 DigitalOut Led19(p25);
vazbyte 0:07212de2fec1 31
vazbyte 19:1998218bbb63 32 DigitalIn pb(p8);
vazbyte 0:07212de2fec1 33
vazbyte 19:1998218bbb63 34 int USV_count = 1;
vazbyte 19:1998218bbb63 35 int old_pb = 1;
vazbyte 19:1998218bbb63 36 int new_pb;
vazbyte 19:1998218bbb63 37
vazbyte 1:df8884d38960 38 const static char DEVICE_NAME[] = "ProVaida";
vazbyte 1:df8884d38960 39 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
vazbyte 19:1998218bbb63 40 short hrmCounter = 0;
vazbyte 19:1998218bbb63 41 static HeartRateService* hrService;
vazbyte 19:1998218bbb63 42
vazbyte 19:1998218bbb63 43 bool BLE_conn = false;
vazbyte 0:07212de2fec1 44
vazbyte 4:af0530752b76 45 int16_t destination[3];
vaida 9:40874a5c5ad0 46 double step_threshold = 14.45;
vazbyte 12:3934b9d6d5a3 47
vaida 9:40874a5c5ad0 48 double oldAcceleration = 0.0;
vazbyte 21:36bd27c83b35 49 int callback_cycles = 1;
vazbyte 13:99ed8a3db67a 50 int step;
vazbyte 22:50f88b18cfad 51 int step_buffer;
vazbyte 19:1998218bbb63 52 int totalsteps = 0;
vazbyte 19:1998218bbb63 53
vazbyte 21:36bd27c83b35 54 int run_threshold = 5;
vaida 15:a502564c7a88 55 int run_count = 0;
vazbyte 10:8cf4069d064a 56
vazbyte 4:af0530752b76 57 MPU9250 mpu = MPU9250(P0_26, P0_27);
vazbyte 0:07212de2fec1 58
vazbyte 19:1998218bbb63 59 static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);
vazbyte 0:07212de2fec1 60
vazbyte 0:07212de2fec1 61 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
vazbyte 0:07212de2fec1 62 {
vazbyte 0:07212de2fec1 63 BLE::Instance().gap().startAdvertising();
vazbyte 0:07212de2fec1 64 }
vazbyte 0:07212de2fec1 65
vazbyte 19:1998218bbb63 66 void updateSensorValue() {
vazbyte 19:1998218bbb63 67 wait(.001);
vazbyte 19:1998218bbb63 68 new_pb = pb;
vazbyte 19:1998218bbb63 69 if ((new_pb == 0) && (old_pb == 1)) {
vazbyte 19:1998218bbb63 70 USV_count++;
vazbyte 19:1998218bbb63 71 printf("count: %i\n", USV_count);
vazbyte 19:1998218bbb63 72 }
vazbyte 19:1998218bbb63 73 if (USV_count % 2) {
vazbyte 19:1998218bbb63 74 step = 0;
vazbyte 19:1998218bbb63 75 callback_cycles++;
vazbyte 13:99ed8a3db67a 76
vazbyte 19:1998218bbb63 77 mpu.readAccelData(destination);
vazbyte 3:a33709dad06c 78
vazbyte 19:1998218bbb63 79 double acc_x = destination[0] / 1000.0;
vazbyte 19:1998218bbb63 80 double acc_y = destination[1] / 1000.0;
vazbyte 19:1998218bbb63 81 double acc_z = destination[2] / 1000.0;
vazbyte 3:a33709dad06c 82
vazbyte 19:1998218bbb63 83 double sqr_acc_x = acc_x*acc_x;
vazbyte 19:1998218bbb63 84 double sqr_acc_y = acc_y*acc_y;
vazbyte 19:1998218bbb63 85 double sqr_acc_z = acc_z*acc_z;
vazbyte 19:1998218bbb63 86
vazbyte 19:1998218bbb63 87 double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
vazbyte 19:1998218bbb63 88 double accel = sqrt(sum_acc);
vazbyte 3:a33709dad06c 89
vazbyte 19:1998218bbb63 90 if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
vazbyte 22:50f88b18cfad 91 step_buffer++;
vazbyte 22:50f88b18cfad 92 //reached running speed
vazbyte 19:1998218bbb63 93 if (callback_cycles <= run_threshold) {
vazbyte 19:1998218bbb63 94 if (run_count >= 2) {
vazbyte 19:1998218bbb63 95 step = 2;
vazbyte 19:1998218bbb63 96 }
vazbyte 19:1998218bbb63 97 else {
vazbyte 19:1998218bbb63 98 step = 1;
vazbyte 19:1998218bbb63 99 run_count++;
vazbyte 19:1998218bbb63 100 }
vaida 15:a502564c7a88 101 }
vazbyte 22:50f88b18cfad 102 //at walking speed
vaida 15:a502564c7a88 103 else {
vaida 16:567f06e22645 104 step = 1;
vazbyte 19:1998218bbb63 105 run_count = 0;
vaida 15:a502564c7a88 106 }
vazbyte 19:1998218bbb63 107 callback_cycles = 0;
vaida 15:a502564c7a88 108 }
vazbyte 19:1998218bbb63 109 if (step != 0)
vazbyte 19:1998218bbb63 110 {
vazbyte 19:1998218bbb63 111 totalsteps++;
vazbyte 19:1998218bbb63 112 Led0 = ((totalsteps >> 0) % 2);
vazbyte 19:1998218bbb63 113 Led1 = ((totalsteps >> 1) % 2);
vazbyte 19:1998218bbb63 114 Led2 = ((totalsteps >> 2) % 2);
vazbyte 19:1998218bbb63 115 Led3 = ((totalsteps >> 3) % 2);
vazbyte 19:1998218bbb63 116 Led4 = ((totalsteps >> 4) % 2);
vazbyte 19:1998218bbb63 117 Led5 = ((totalsteps >> 5) % 2);
vazbyte 19:1998218bbb63 118 Led6 = ((totalsteps >> 6) % 2);
vazbyte 19:1998218bbb63 119 Led7 = ((totalsteps >> 7) % 2);
vazbyte 19:1998218bbb63 120 Led8 = ((totalsteps >> 8) % 2);
vazbyte 19:1998218bbb63 121 Led9 = ((totalsteps >> 9) % 2);
vazbyte 19:1998218bbb63 122 Led10 = ((totalsteps >> 10) % 2);
vazbyte 19:1998218bbb63 123 Led11 = ((totalsteps >> 11) % 2);
vazbyte 19:1998218bbb63 124 Led12 = ((totalsteps >> 12) % 2);
vazbyte 19:1998218bbb63 125 Led13 = ((totalsteps >> 12) % 2);
vazbyte 19:1998218bbb63 126 Led14 = ((totalsteps >> 14) % 2);
vazbyte 19:1998218bbb63 127 Led15 = ((totalsteps >> 15) % 2);
vazbyte 19:1998218bbb63 128 Led16 = ((totalsteps >> 16) % 2);
vazbyte 19:1998218bbb63 129 Led17 = ((totalsteps >> 17) % 2);
vazbyte 19:1998218bbb63 130 Led18 = ((totalsteps >> 18) % 2);
vazbyte 21:36bd27c83b35 131 Led19 = (step == 2);
vazbyte 21:36bd27c83b35 132
vazbyte 21:36bd27c83b35 133 if ((totalsteps >> 19) % 2) {
vazbyte 19:1998218bbb63 134 totalsteps = 0;
vazbyte 19:1998218bbb63 135 }
vazbyte 19:1998218bbb63 136 }
vazbyte 19:1998218bbb63 137 oldAcceleration = accel;
vazbyte 17:b4a080229f5c 138
vazbyte 19:1998218bbb63 139 } else if (totalsteps != 0) {
vazbyte 19:1998218bbb63 140 totalsteps = 0;
vazbyte 19:1998218bbb63 141 }
vazbyte 19:1998218bbb63 142 old_pb = new_pb;
vazbyte 19:1998218bbb63 143
vazbyte 19:1998218bbb63 144 if (BLE_conn) {
vazbyte 22:50f88b18cfad 145 step_buffer--;
vazbyte 22:50f88b18cfad 146 if (step_buffer > 0) {
vazbyte 22:50f88b18cfad 147 hrService->updateHeartRate(-1);
vazbyte 22:50f88b18cfad 148 while (step_buffer > 127) {
vazbyte 22:50f88b18cfad 149 hrService->updateHeartRate(127);
vazbyte 22:50f88b18cfad 150 step_buffer -= 127;
vazbyte 22:50f88b18cfad 151 }
vazbyte 22:50f88b18cfad 152 hrService->updateHeartRate(step_buffer);
vazbyte 22:50f88b18cfad 153 step_buffer = 0;
vazbyte 22:50f88b18cfad 154 hrService->updateHeartRate(-1);
vazbyte 22:50f88b18cfad 155 }
vazbyte 19:1998218bbb63 156 hrmCounter = (short) step;
vazbyte 22:50f88b18cfad 157 hrService->updateHeartRate(hrmCounter);
vazbyte 21:36bd27c83b35 158 BLE_conn = false;
vazbyte 17:b4a080229f5c 159 }
vazbyte 0:07212de2fec1 160 }
vazbyte 0:07212de2fec1 161
vazbyte 0:07212de2fec1 162 void blinkCallback(void)
vazbyte 0:07212de2fec1 163 {
vazbyte 0:07212de2fec1 164 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 165 if (ble.gap().getState().connected) {
vazbyte 21:36bd27c83b35 166 BLE_conn = true;
vazbyte 0:07212de2fec1 167 }
vazbyte 21:36bd27c83b35 168 eventQueue.call(updateSensorValue);
vazbyte 21:36bd27c83b35 169
vazbyte 0:07212de2fec1 170 }
vazbyte 0:07212de2fec1 171
vazbyte 19:1998218bbb63 172 void onBleInitError(BLE &ble, ble_error_t error) {}
vazbyte 0:07212de2fec1 173
vazbyte 0:07212de2fec1 174 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
vazbyte 0:07212de2fec1 175 {
vazbyte 0:07212de2fec1 176 BLE& ble = params->ble;
vazbyte 0:07212de2fec1 177 ble_error_t error = params->error;
vazbyte 0:07212de2fec1 178
vazbyte 19:1998218bbb63 179 if (error != BLE_ERROR_NONE) {
vazbyte 0:07212de2fec1 180 onBleInitError(ble, error);
vazbyte 0:07212de2fec1 181 return;
vazbyte 0:07212de2fec1 182 }
vazbyte 0:07212de2fec1 183
vazbyte 0:07212de2fec1 184 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
vazbyte 0:07212de2fec1 185 return;
vazbyte 0:07212de2fec1 186 }
vazbyte 0:07212de2fec1 187
vazbyte 0:07212de2fec1 188 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 0:07212de2fec1 189
vazbyte 1:df8884d38960 190 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 0:07212de2fec1 191
vazbyte 0:07212de2fec1 192 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 0:07212de2fec1 193 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
vazbyte 0:07212de2fec1 194 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 0:07212de2fec1 195 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 19:1998218bbb63 196 ble.gap().setAdvertisingInterval(1000);
vazbyte 0:07212de2fec1 197 ble.gap().startAdvertising();
vazbyte 0:07212de2fec1 198 }
vazbyte 0:07212de2fec1 199
vazbyte 0:07212de2fec1 200 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
vazbyte 0:07212de2fec1 201 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 202 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
vazbyte 0:07212de2fec1 203 }
vazbyte 0:07212de2fec1 204
vazbyte 0:07212de2fec1 205 int main()
vazbyte 19:1998218bbb63 206 {
vazbyte 19:1998218bbb63 207
vazbyte 19:1998218bbb63 208 pb.mode(PullUp);
vazbyte 2:1957a4985d6e 209 mpu.initMPU9250();
vazbyte 10:8cf4069d064a 210
vazbyte 21:36bd27c83b35 211 eventQueue.call_every(80, blinkCallback);
vazbyte 0:07212de2fec1 212
vazbyte 21:36bd27c83b35 213 BLE &ble = BLE::Instance();
vazbyte 21:36bd27c83b35 214 ble.onEventsToProcess(scheduleBleEventsProcessing);
vazbyte 21:36bd27c83b35 215 ble.init(bleInitComplete);
vazbyte 21:36bd27c83b35 216
vazbyte 21:36bd27c83b35 217 eventQueue.dispatch_forever();
vazbyte 0:07212de2fec1 218
vazbyte 0:07212de2fec1 219 return 0;
vazbyte 0:07212de2fec1 220 }