Nora Vazbyte / Mbed OS 99Problems-BLEAint1

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

source/main.cpp@19:1998218bbb63, 2018-11-27 (annotated)

Committer:
vazbyte
Date:
Tue Nov 27 17:01:37 2018 +0000
Revision:
19:1998218bbb63
Parent:
18:4cbd88c0a374
Child:
21:36bd27c83b35
App critics will say it's money, cash, toes

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 19:1998218bbb63 11 DigitalOut Led0(p3);
vazbyte 19:1998218bbb63 12 DigitalOut Led1(p4);
vazbyte 19:1998218bbb63 13 DigitalOut Led2(p28);
vazbyte 19:1998218bbb63 14 DigitalOut Led3(p29);
vazbyte 19:1998218bbb63 15 DigitalOut Led4(p30);
vazbyte 19:1998218bbb63 16 DigitalOut Led5(p31);
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;
vaida 15:a502564c7a88 49 int callback_cycles = 1; // used to be 4
vazbyte 13:99ed8a3db67a 50 int step;
vazbyte 19:1998218bbb63 51 int totalsteps = 0;
vazbyte 19:1998218bbb63 52
vaida 15:a502564c7a88 53 int run_threshold = 5; // used to be 7
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 19:1998218bbb63 127 Led19 = ((totalsteps >> 19) % 2);
vazbyte 19:1998218bbb63 128 if ((totalsteps >> 20) % 2 == 1) {
vazbyte 19:1998218bbb63 129 totalsteps = 0;
vazbyte 19:1998218bbb63 130 }
vazbyte 19:1998218bbb63 131 }
vazbyte 19:1998218bbb63 132 oldAcceleration = accel;
vazbyte 17:b4a080229f5c 133
vazbyte 19:1998218bbb63 134 } else if (totalsteps != 0) {
vazbyte 19:1998218bbb63 135 totalsteps = 0;
vazbyte 19:1998218bbb63 136 }
vazbyte 19:1998218bbb63 137 old_pb = new_pb;
vazbyte 19:1998218bbb63 138
vazbyte 19:1998218bbb63 139 if (BLE_conn) {
vazbyte 19:1998218bbb63 140 hrmCounter = (short) step;
vazbyte 19:1998218bbb63 141 hrService->updateHeartRate(hrmCounter);
vazbyte 17:b4a080229f5c 142 }
vazbyte 0:07212de2fec1 143 }
vazbyte 0:07212de2fec1 144
vazbyte 0:07212de2fec1 145 void blinkCallback(void)
vazbyte 0:07212de2fec1 146 {
vazbyte 0:07212de2fec1 147 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 148 if (ble.gap().getState().connected) {
vazbyte 0:07212de2fec1 149 eventQueue.call(updateSensorValue);
vazbyte 0:07212de2fec1 150 }
vazbyte 0:07212de2fec1 151 }
vazbyte 0:07212de2fec1 152
vazbyte 19:1998218bbb63 153 void onBleInitError(BLE &ble, ble_error_t error) {}
vazbyte 0:07212de2fec1 154
vazbyte 0:07212de2fec1 155 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
vazbyte 0:07212de2fec1 156 {
vazbyte 0:07212de2fec1 157 BLE& ble = params->ble;
vazbyte 0:07212de2fec1 158 ble_error_t error = params->error;
vazbyte 0:07212de2fec1 159
vazbyte 19:1998218bbb63 160 if (error != BLE_ERROR_NONE) {
vazbyte 0:07212de2fec1 161 onBleInitError(ble, error);
vazbyte 0:07212de2fec1 162 return;
vazbyte 0:07212de2fec1 163 }
vazbyte 0:07212de2fec1 164
vazbyte 0:07212de2fec1 165 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
vazbyte 0:07212de2fec1 166 return;
vazbyte 0:07212de2fec1 167 }
vazbyte 0:07212de2fec1 168
vazbyte 0:07212de2fec1 169 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 0:07212de2fec1 170
vazbyte 1:df8884d38960 171 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 0:07212de2fec1 172
vazbyte 0:07212de2fec1 173 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 0:07212de2fec1 174 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
vazbyte 0:07212de2fec1 175 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 0:07212de2fec1 176 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 19:1998218bbb63 177 ble.gap().setAdvertisingInterval(1000);
vazbyte 0:07212de2fec1 178 ble.gap().startAdvertising();
vazbyte 0:07212de2fec1 179 }
vazbyte 0:07212de2fec1 180
vazbyte 0:07212de2fec1 181 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
vazbyte 0:07212de2fec1 182 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 183 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
vazbyte 0:07212de2fec1 184 }
vazbyte 0:07212de2fec1 185
vazbyte 0:07212de2fec1 186 int main()
vazbyte 19:1998218bbb63 187 {
vazbyte 19:1998218bbb63 188
vazbyte 19:1998218bbb63 189 pb.mode(PullUp);
vazbyte 2:1957a4985d6e 190 mpu.initMPU9250();
vazbyte 10:8cf4069d064a 191
vazbyte 19:1998218bbb63 192 while (1) {
vazbyte 19:1998218bbb63 193 updateSensorValue();
vazbyte 19:1998218bbb63 194
vazbyte 19:1998218bbb63 195 BLE &ble = BLE::Instance();
vazbyte 19:1998218bbb63 196 ble.onEventsToProcess(scheduleBleEventsProcessing);
vazbyte 19:1998218bbb63 197 ble.init(bleInitComplete);
vazbyte 19:1998218bbb63 198
vazbyte 19:1998218bbb63 199 if (ble.gap().getState().connected) {
vazbyte 19:1998218bbb63 200 BLE_conn = true;
vazbyte 19:1998218bbb63 201 //eventQueue.call(updateSensorValue);
vazbyte 19:1998218bbb63 202 }
vazbyte 0:07212de2fec1 203
vazbyte 19:1998218bbb63 204 //eventQueue.dispatch_forever();
vazbyte 19:1998218bbb63 205
vazbyte 19:1998218bbb63 206 wait(0.1);
vazbyte 19:1998218bbb63 207 }
vazbyte 0:07212de2fec1 208
vazbyte 0:07212de2fec1 209 return 0;
vazbyte 0:07212de2fec1 210 }