Nora Vazbyte / Mbed OS 99Problems-BLEAint1

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

source/main.cpp@21:36bd27c83b35, 2018-12-14 (annotated)

Committer:
vazbyte
Date:
Fri Dec 14 16:22:10 2018 +0000
Revision:
21:36bd27c83b35
Parent:
19:1998218bbb63
Child:
22:50f88b18cfad
repooo

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 19:1998218bbb63 51 int totalsteps = 0;
vazbyte 19:1998218bbb63 52
vazbyte 21:36bd27c83b35 53 int run_threshold = 5;
vaida 15:a502564c7a88 54 int run_count = 0;
vazbyte 10:8cf4069d064a 55
vazbyte 4:af0530752b76 56 MPU9250 mpu = MPU9250(P0_26, P0_27);
vazbyte 0:07212de2fec1 57
vazbyte 19:1998218bbb63 58 static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);
vazbyte 0:07212de2fec1 59
vazbyte 0:07212de2fec1 60 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
vazbyte 0:07212de2fec1 61 {
vazbyte 0:07212de2fec1 62 BLE::Instance().gap().startAdvertising();
vazbyte 0:07212de2fec1 63 }
vazbyte 0:07212de2fec1 64
vazbyte 19:1998218bbb63 65 void updateSensorValue() {
vazbyte 19:1998218bbb63 66 wait(.001);
vazbyte 19:1998218bbb63 67 new_pb = pb;
vazbyte 19:1998218bbb63 68 if ((new_pb == 0) && (old_pb == 1)) {
vazbyte 19:1998218bbb63 69 USV_count++;
vazbyte 19:1998218bbb63 70 printf("count: %i\n", USV_count);
vazbyte 19:1998218bbb63 71 }
vazbyte 19:1998218bbb63 72 if (USV_count % 2) {
vazbyte 19:1998218bbb63 73 step = 0;
vazbyte 19:1998218bbb63 74 callback_cycles++;
vazbyte 13:99ed8a3db67a 75
vazbyte 19:1998218bbb63 76 mpu.readAccelData(destination);
vazbyte 3:a33709dad06c 77
vazbyte 19:1998218bbb63 78 double acc_x = destination[0] / 1000.0;
vazbyte 19:1998218bbb63 79 double acc_y = destination[1] / 1000.0;
vazbyte 19:1998218bbb63 80 double acc_z = destination[2] / 1000.0;
vazbyte 3:a33709dad06c 81
vazbyte 19:1998218bbb63 82 double sqr_acc_x = acc_x*acc_x;
vazbyte 19:1998218bbb63 83 double sqr_acc_y = acc_y*acc_y;
vazbyte 19:1998218bbb63 84 double sqr_acc_z = acc_z*acc_z;
vazbyte 19:1998218bbb63 85
vazbyte 19:1998218bbb63 86 double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
vazbyte 19:1998218bbb63 87 double accel = sqrt(sum_acc);
vazbyte 3:a33709dad06c 88
vazbyte 19:1998218bbb63 89 if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
vazbyte 19:1998218bbb63 90 if (callback_cycles <= run_threshold) {
vazbyte 19:1998218bbb63 91 if (run_count >= 2) {
vazbyte 19:1998218bbb63 92 step = 2;
vazbyte 19:1998218bbb63 93 }
vazbyte 19:1998218bbb63 94 else {
vazbyte 19:1998218bbb63 95 step = 1;
vazbyte 19:1998218bbb63 96 run_count++;
vazbyte 19:1998218bbb63 97 }
vaida 15:a502564c7a88 98 }
vaida 15:a502564c7a88 99 else {
vaida 16:567f06e22645 100 step = 1;
vazbyte 19:1998218bbb63 101 run_count = 0;
vaida 15:a502564c7a88 102 }
vazbyte 19:1998218bbb63 103 callback_cycles = 0;
vaida 15:a502564c7a88 104 }
vazbyte 19:1998218bbb63 105 if (step != 0)
vazbyte 19:1998218bbb63 106 {
vazbyte 19:1998218bbb63 107 totalsteps++;
vazbyte 19:1998218bbb63 108 Led0 = ((totalsteps >> 0) % 2);
vazbyte 19:1998218bbb63 109 Led1 = ((totalsteps >> 1) % 2);
vazbyte 19:1998218bbb63 110 Led2 = ((totalsteps >> 2) % 2);
vazbyte 19:1998218bbb63 111 Led3 = ((totalsteps >> 3) % 2);
vazbyte 19:1998218bbb63 112 Led4 = ((totalsteps >> 4) % 2);
vazbyte 19:1998218bbb63 113 Led5 = ((totalsteps >> 5) % 2);
vazbyte 19:1998218bbb63 114 Led6 = ((totalsteps >> 6) % 2);
vazbyte 19:1998218bbb63 115 Led7 = ((totalsteps >> 7) % 2);
vazbyte 19:1998218bbb63 116 Led8 = ((totalsteps >> 8) % 2);
vazbyte 19:1998218bbb63 117 Led9 = ((totalsteps >> 9) % 2);
vazbyte 19:1998218bbb63 118 Led10 = ((totalsteps >> 10) % 2);
vazbyte 19:1998218bbb63 119 Led11 = ((totalsteps >> 11) % 2);
vazbyte 19:1998218bbb63 120 Led12 = ((totalsteps >> 12) % 2);
vazbyte 19:1998218bbb63 121 Led13 = ((totalsteps >> 12) % 2);
vazbyte 19:1998218bbb63 122 Led14 = ((totalsteps >> 14) % 2);
vazbyte 19:1998218bbb63 123 Led15 = ((totalsteps >> 15) % 2);
vazbyte 19:1998218bbb63 124 Led16 = ((totalsteps >> 16) % 2);
vazbyte 19:1998218bbb63 125 Led17 = ((totalsteps >> 17) % 2);
vazbyte 19:1998218bbb63 126 Led18 = ((totalsteps >> 18) % 2);
vazbyte 21:36bd27c83b35 127 Led19 = (step == 2);
vazbyte 21:36bd27c83b35 128
vazbyte 21:36bd27c83b35 129 if ((totalsteps >> 19) % 2) {
vazbyte 19:1998218bbb63 130 totalsteps = 0;
vazbyte 19:1998218bbb63 131 }
vazbyte 19:1998218bbb63 132 }
vazbyte 19:1998218bbb63 133 oldAcceleration = accel;
vazbyte 17:b4a080229f5c 134
vazbyte 19:1998218bbb63 135 } else if (totalsteps != 0) {
vazbyte 19:1998218bbb63 136 totalsteps = 0;
vazbyte 19:1998218bbb63 137 }
vazbyte 19:1998218bbb63 138 old_pb = new_pb;
vazbyte 19:1998218bbb63 139
vazbyte 19:1998218bbb63 140 if (BLE_conn) {
vazbyte 19:1998218bbb63 141 hrmCounter = (short) step;
vazbyte 19:1998218bbb63 142 hrService->updateHeartRate(hrmCounter);
vazbyte 21:36bd27c83b35 143 BLE_conn = false;
vazbyte 17:b4a080229f5c 144 }
vazbyte 0:07212de2fec1 145 }
vazbyte 0:07212de2fec1 146
vazbyte 0:07212de2fec1 147 void blinkCallback(void)
vazbyte 0:07212de2fec1 148 {
vazbyte 0:07212de2fec1 149 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 150 if (ble.gap().getState().connected) {
vazbyte 21:36bd27c83b35 151 BLE_conn = true;
vazbyte 0:07212de2fec1 152 }
vazbyte 21:36bd27c83b35 153 eventQueue.call(updateSensorValue);
vazbyte 21:36bd27c83b35 154
vazbyte 0:07212de2fec1 155 }
vazbyte 0:07212de2fec1 156
vazbyte 19:1998218bbb63 157 void onBleInitError(BLE &ble, ble_error_t error) {}
vazbyte 0:07212de2fec1 158
vazbyte 0:07212de2fec1 159 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
vazbyte 0:07212de2fec1 160 {
vazbyte 0:07212de2fec1 161 BLE& ble = params->ble;
vazbyte 0:07212de2fec1 162 ble_error_t error = params->error;
vazbyte 0:07212de2fec1 163
vazbyte 19:1998218bbb63 164 if (error != BLE_ERROR_NONE) {
vazbyte 0:07212de2fec1 165 onBleInitError(ble, error);
vazbyte 0:07212de2fec1 166 return;
vazbyte 0:07212de2fec1 167 }
vazbyte 0:07212de2fec1 168
vazbyte 0:07212de2fec1 169 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
vazbyte 0:07212de2fec1 170 return;
vazbyte 0:07212de2fec1 171 }
vazbyte 0:07212de2fec1 172
vazbyte 0:07212de2fec1 173 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 0:07212de2fec1 174
vazbyte 1:df8884d38960 175 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 0:07212de2fec1 176
vazbyte 0:07212de2fec1 177 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 0:07212de2fec1 178 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
vazbyte 0:07212de2fec1 179 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 0:07212de2fec1 180 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 19:1998218bbb63 181 ble.gap().setAdvertisingInterval(1000);
vazbyte 0:07212de2fec1 182 ble.gap().startAdvertising();
vazbyte 0:07212de2fec1 183 }
vazbyte 0:07212de2fec1 184
vazbyte 0:07212de2fec1 185 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
vazbyte 0:07212de2fec1 186 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 187 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
vazbyte 0:07212de2fec1 188 }
vazbyte 0:07212de2fec1 189
vazbyte 0:07212de2fec1 190 int main()
vazbyte 19:1998218bbb63 191 {
vazbyte 19:1998218bbb63 192
vazbyte 19:1998218bbb63 193 pb.mode(PullUp);
vazbyte 2:1957a4985d6e 194 mpu.initMPU9250();
vazbyte 10:8cf4069d064a 195
vazbyte 21:36bd27c83b35 196 eventQueue.call_every(80, blinkCallback);
vazbyte 0:07212de2fec1 197
vazbyte 21:36bd27c83b35 198 BLE &ble = BLE::Instance();
vazbyte 21:36bd27c83b35 199 ble.onEventsToProcess(scheduleBleEventsProcessing);
vazbyte 21:36bd27c83b35 200 ble.init(bleInitComplete);
vazbyte 21:36bd27c83b35 201
vazbyte 21:36bd27c83b35 202 eventQueue.dispatch_forever();
vazbyte 0:07212de2fec1 203
vazbyte 0:07212de2fec1 204 return 0;
vazbyte 0:07212de2fec1 205 }