Nora Vazbyte / Mbed OS 99Problems-BLEAint1

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

source/main.cpp@23:7126ef3503e2, 2018-12-16 (annotated)

Committer:
vazbyte
Date:
Sun Dec 16 14:35:50 2018 +0000
Revision:
23:7126ef3503e2
Parent:
22:50f88b18cfad 
now step_buffer resets on button press

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 23:7126ef3503e2 141 step_buffer = 0;
vazbyte 19:1998218bbb63 142 }
vazbyte 19:1998218bbb63 143 old_pb = new_pb;
vazbyte 19:1998218bbb63 144
vazbyte 19:1998218bbb63 145 if (BLE_conn) {
vazbyte 22:50f88b18cfad 146 step_buffer--;
vazbyte 22:50f88b18cfad 147 if (step_buffer > 0) {
vazbyte 22:50f88b18cfad 148 hrService->updateHeartRate(-1);
vazbyte 22:50f88b18cfad 149 while (step_buffer > 127) {
vazbyte 22:50f88b18cfad 150 hrService->updateHeartRate(127);
vazbyte 22:50f88b18cfad 151 step_buffer -= 127;
vazbyte 22:50f88b18cfad 152 }
vazbyte 22:50f88b18cfad 153 hrService->updateHeartRate(step_buffer);
vazbyte 22:50f88b18cfad 154 step_buffer = 0;
vazbyte 22:50f88b18cfad 155 hrService->updateHeartRate(-1);
vazbyte 22:50f88b18cfad 156 }
vazbyte 19:1998218bbb63 157 hrmCounter = (short) step;
vazbyte 22:50f88b18cfad 158 hrService->updateHeartRate(hrmCounter);
vazbyte 21:36bd27c83b35 159 BLE_conn = false;
vazbyte 17:b4a080229f5c 160 }
vazbyte 0:07212de2fec1 161 }
vazbyte 0:07212de2fec1 162
vazbyte 0:07212de2fec1 163 void blinkCallback(void)
vazbyte 0:07212de2fec1 164 {
vazbyte 0:07212de2fec1 165 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 166 if (ble.gap().getState().connected) {
vazbyte 21:36bd27c83b35 167 BLE_conn = true;
vazbyte 0:07212de2fec1 168 }
vazbyte 21:36bd27c83b35 169 eventQueue.call(updateSensorValue);
vazbyte 21:36bd27c83b35 170
vazbyte 0:07212de2fec1 171 }
vazbyte 0:07212de2fec1 172
vazbyte 19:1998218bbb63 173 void onBleInitError(BLE &ble, ble_error_t error) {}
vazbyte 0:07212de2fec1 174
vazbyte 0:07212de2fec1 175 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
vazbyte 0:07212de2fec1 176 {
vazbyte 0:07212de2fec1 177 BLE& ble = params->ble;
vazbyte 0:07212de2fec1 178 ble_error_t error = params->error;
vazbyte 0:07212de2fec1 179
vazbyte 19:1998218bbb63 180 if (error != BLE_ERROR_NONE) {
vazbyte 0:07212de2fec1 181 onBleInitError(ble, error);
vazbyte 0:07212de2fec1 182 return;
vazbyte 0:07212de2fec1 183 }
vazbyte 0:07212de2fec1 184
vazbyte 0:07212de2fec1 185 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
vazbyte 0:07212de2fec1 186 return;
vazbyte 0:07212de2fec1 187 }
vazbyte 0:07212de2fec1 188
vazbyte 0:07212de2fec1 189 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 0:07212de2fec1 190
vazbyte 1:df8884d38960 191 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 0:07212de2fec1 192
vazbyte 0:07212de2fec1 193 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 0:07212de2fec1 194 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
vazbyte 0:07212de2fec1 195 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 0:07212de2fec1 196 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 19:1998218bbb63 197 ble.gap().setAdvertisingInterval(1000);
vazbyte 0:07212de2fec1 198 ble.gap().startAdvertising();
vazbyte 0:07212de2fec1 199 }
vazbyte 0:07212de2fec1 200
vazbyte 0:07212de2fec1 201 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
vazbyte 0:07212de2fec1 202 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 203 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
vazbyte 0:07212de2fec1 204 }
vazbyte 0:07212de2fec1 205
vazbyte 0:07212de2fec1 206 int main()
vazbyte 19:1998218bbb63 207 {
vazbyte 19:1998218bbb63 208
vazbyte 19:1998218bbb63 209 pb.mode(PullUp);
vazbyte 2:1957a4985d6e 210 mpu.initMPU9250();
vazbyte 10:8cf4069d064a 211
vazbyte 21:36bd27c83b35 212 eventQueue.call_every(80, blinkCallback);
vazbyte 0:07212de2fec1 213
vazbyte 21:36bd27c83b35 214 BLE &ble = BLE::Instance();
vazbyte 21:36bd27c83b35 215 ble.onEventsToProcess(scheduleBleEventsProcessing);
vazbyte 21:36bd27c83b35 216 ble.init(bleInitComplete);
vazbyte 21:36bd27c83b35 217
vazbyte 21:36bd27c83b35 218 eventQueue.dispatch_forever();
vazbyte 0:07212de2fec1 219
vazbyte 0:07212de2fec1 220 return 0;
vazbyte 0:07212de2fec1 221 }