Nora Vazbyte / Mbed OS 99Problems-BLEAint1

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

source/main.cpp@16:567f06e22645, 2018-11-21 (annotated)

Committer:
vaida
Date:
Wed Nov 21 10:25:58 2018 +0000
Revision:
16:567f06e22645
Parent:
15:a502564c7a88
Child:
17:b4a080229f5c
fixed undercounting walking (skipped counting if below run_count)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vazbyte 0:07212de2fec1 1 /* mbed Microcontroller Library
vazbyte 0:07212de2fec1 2 * Copyright (c) 2006-2014 ARM Limited
vazbyte 0:07212de2fec1 3 *
vazbyte 0:07212de2fec1 4 * Licensed under the Apache License, Version 2.0 (the "License");
vazbyte 0:07212de2fec1 5 * you may not use this file except in compliance with the License.
vazbyte 0:07212de2fec1 6 * You may obtain a copy of the License at
vazbyte 0:07212de2fec1 7 *
vazbyte 0:07212de2fec1 8 * http://www.apache.org/licenses/LICENSE-2.0
vazbyte 0:07212de2fec1 9 *
vazbyte 0:07212de2fec1 10 * Unless required by applicable law or agreed to in writing, software
vazbyte 0:07212de2fec1 11 * distributed under the License is distributed on an "AS IS" BASIS,
vazbyte 0:07212de2fec1 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
vazbyte 0:07212de2fec1 13 * See the License for the specific language governing permissions and
vazbyte 0:07212de2fec1 14 * limitations under the License.
vazbyte 0:07212de2fec1 15 */
vazbyte 0:07212de2fec1 16
vazbyte 0:07212de2fec1 17 #include <events/mbed_events.h>
vazbyte 3:a33709dad06c 18 #include <math.h>
vazbyte 11:5023e8c93e4d 19
vazbyte 10:8cf4069d064a 20 #include <ctime>
vazbyte 0:07212de2fec1 21 #include <mbed.h>
vazbyte 1:df8884d38960 22 #include "MPU9250.h"
vazbyte 0:07212de2fec1 23 #include "ble/BLE.h"
vazbyte 0:07212de2fec1 24 #include "ble/Gap.h"
vazbyte 1:df8884d38960 25 #include "ble/services/HeartRateService.h"
vazbyte 1:df8884d38960 26
vazbyte 0:07212de2fec1 27
vazbyte 0:07212de2fec1 28 DigitalOut led1(LED1, 1);
vazbyte 0:07212de2fec1 29
vazbyte 1:df8884d38960 30 const static char DEVICE_NAME[] = "ProVaida";
vazbyte 1:df8884d38960 31 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
vazbyte 0:07212de2fec1 32
vazbyte 4:af0530752b76 33 int16_t destination[3];
vaida 8:1735fddd5491 34 short hrmCounter = 0;
vaida 9:40874a5c5ad0 35 double step_threshold = 14.45;
vazbyte 12:3934b9d6d5a3 36
vaida 9:40874a5c5ad0 37 double oldAcceleration = 0.0;
vaida 15:a502564c7a88 38 int callback_cycles = 1; // used to be 4
vazbyte 13:99ed8a3db67a 39 int step;
vaida 15:a502564c7a88 40 int run_threshold = 5; // used to be 7
vaida 15:a502564c7a88 41 int run_count = 0;
vazbyte 10:8cf4069d064a 42
vazbyte 1:df8884d38960 43 static HeartRateService* hrService;
vazbyte 4:af0530752b76 44 MPU9250 mpu = MPU9250(P0_26, P0_27);
vazbyte 0:07212de2fec1 45
vazbyte 0:07212de2fec1 46 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
vazbyte 0:07212de2fec1 47
vazbyte 0:07212de2fec1 48 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
vazbyte 0:07212de2fec1 49 {
vazbyte 0:07212de2fec1 50 BLE::Instance().gap().startAdvertising();
vazbyte 0:07212de2fec1 51 }
vazbyte 0:07212de2fec1 52
vazbyte 2:1957a4985d6e 53 void updateSensorValue() {
vazbyte 13:99ed8a3db67a 54 step = 0;
vazbyte 13:99ed8a3db67a 55 callback_cycles++;
vazbyte 13:99ed8a3db67a 56
vazbyte 3:a33709dad06c 57 mpu.readAccelData(destination);
vazbyte 3:a33709dad06c 58
vaida 8:1735fddd5491 59 double acc_x = destination[0] / 1000.0;
vaida 8:1735fddd5491 60 double acc_y = destination[1] / 1000.0;
vaida 8:1735fddd5491 61 double acc_z = destination[2] / 1000.0;
vazbyte 3:a33709dad06c 62
vazbyte 7:9e2172e6550a 63 double sqr_acc_x = acc_x*acc_x;
vazbyte 7:9e2172e6550a 64 double sqr_acc_y = acc_y*acc_y;
vazbyte 7:9e2172e6550a 65 double sqr_acc_z = acc_z*acc_z;
vazbyte 3:a33709dad06c 66
vazbyte 3:a33709dad06c 67 double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
vaida 9:40874a5c5ad0 68 double accel = sqrt(sum_acc);
vazbyte 11:5023e8c93e4d 69
vazbyte 14:aa0029dbb3e2 70 //printf("calback cycles: " );
vazbyte 14:aa0029dbb3e2 71 //printf("%i\n", callback_cycles);
vazbyte 10:8cf4069d064a 72
vazbyte 14:aa0029dbb3e2 73 if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
vaida 15:a502564c7a88 74 if (callback_cycles <= run_threshold) {
vaida 15:a502564c7a88 75 if (run_count >= 2) {
vaida 15:a502564c7a88 76 step = 2;
vaida 15:a502564c7a88 77 }
vaida 15:a502564c7a88 78 else {
vaida 16:567f06e22645 79 step = 1;
vaida 15:a502564c7a88 80 run_count++;
vaida 15:a502564c7a88 81 }
vaida 15:a502564c7a88 82 }
vaida 15:a502564c7a88 83 else {
vaida 15:a502564c7a88 84 step = 1;
vaida 15:a502564c7a88 85 run_count = 0;
vaida 15:a502564c7a88 86 }
vazbyte 13:99ed8a3db67a 87 callback_cycles = 0;
vazbyte 10:8cf4069d064a 88 }
vaida 9:40874a5c5ad0 89
vaida 9:40874a5c5ad0 90 oldAcceleration = accel;
vaida 9:40874a5c5ad0 91
vaida 9:40874a5c5ad0 92 hrmCounter = (short) step;
vazbyte 14:aa0029dbb3e2 93 //printf("STEP: " );
vazbyte 14:aa0029dbb3e2 94 //printf("%hu\n", hrmCounter);
vaida 9:40874a5c5ad0 95
vazbyte 1:df8884d38960 96 hrService->updateHeartRate(hrmCounter);
vazbyte 0:07212de2fec1 97 }
vazbyte 0:07212de2fec1 98
vazbyte 0:07212de2fec1 99 void blinkCallback(void)
vazbyte 0:07212de2fec1 100 {
vazbyte 0:07212de2fec1 101 led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
vazbyte 0:07212de2fec1 102 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 103 if (ble.gap().getState().connected) {
vazbyte 0:07212de2fec1 104 eventQueue.call(updateSensorValue);
vazbyte 0:07212de2fec1 105 }
vazbyte 0:07212de2fec1 106 }
vazbyte 0:07212de2fec1 107
vazbyte 0:07212de2fec1 108 /**
vazbyte 1:df8884d38960 109 * This function is called when the ble initialization process has failed
vazbyte 0:07212de2fec1 110 */
vazbyte 0:07212de2fec1 111 void onBleInitError(BLE &ble, ble_error_t error)
vazbyte 0:07212de2fec1 112 {
vazbyte 0:07212de2fec1 113 /* Initialization error handling should go here */
vazbyte 0:07212de2fec1 114 }
vazbyte 0:07212de2fec1 115
vazbyte 0:07212de2fec1 116 void printMacAddress()
vazbyte 0:07212de2fec1 117 {
vazbyte 0:07212de2fec1 118 /* Print out device MAC address to the console*/
vazbyte 0:07212de2fec1 119 Gap::AddressType_t addr_type;
vazbyte 0:07212de2fec1 120 Gap::Address_t address;
vazbyte 0:07212de2fec1 121 BLE::Instance().gap().getAddress(&addr_type, address);
vazbyte 0:07212de2fec1 122 printf("DEVICE MAC ADDRESS: ");
vazbyte 0:07212de2fec1 123 for (int i = 5; i >= 1; i--){
vazbyte 0:07212de2fec1 124 printf("%02x:", address[i]);
vazbyte 0:07212de2fec1 125 }
vazbyte 0:07212de2fec1 126 printf("%02x\r\n", address[0]);
vazbyte 0:07212de2fec1 127 }
vazbyte 0:07212de2fec1 128
vazbyte 0:07212de2fec1 129 /**
vazbyte 0:07212de2fec1 130 * Callback triggered when the ble initialization process has finished
vazbyte 0:07212de2fec1 131 */
vazbyte 0:07212de2fec1 132 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
vazbyte 0:07212de2fec1 133 {
vazbyte 0:07212de2fec1 134 BLE& ble = params->ble;
vazbyte 0:07212de2fec1 135 ble_error_t error = params->error;
vazbyte 0:07212de2fec1 136
vazbyte 0:07212de2fec1 137 if (error != BLE_ERROR_NONE) {
vazbyte 0:07212de2fec1 138 /* In case of error, forward the error handling to onBleInitError */
vazbyte 0:07212de2fec1 139 onBleInitError(ble, error);
vazbyte 0:07212de2fec1 140 return;
vazbyte 0:07212de2fec1 141 }
vazbyte 0:07212de2fec1 142
vazbyte 0:07212de2fec1 143 /* Ensure that it is the default instance of BLE */
vazbyte 0:07212de2fec1 144 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
vazbyte 0:07212de2fec1 145 return;
vazbyte 0:07212de2fec1 146 }
vazbyte 0:07212de2fec1 147
vazbyte 0:07212de2fec1 148 ble.gap().onDisconnection(disconnectionCallback);
vazbyte 0:07212de2fec1 149
vazbyte 0:07212de2fec1 150 /* Setup primary service */
vazbyte 1:df8884d38960 151 hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
vazbyte 0:07212de2fec1 152
vazbyte 0:07212de2fec1 153 /* Setup advertising */
vazbyte 0:07212de2fec1 154 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
vazbyte 0:07212de2fec1 155 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
vazbyte 0:07212de2fec1 156 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
vazbyte 0:07212de2fec1 157 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
vazbyte 0:07212de2fec1 158 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
vazbyte 0:07212de2fec1 159 ble.gap().startAdvertising();
vazbyte 0:07212de2fec1 160
vaida 6:887df9bbe705 161
vazbyte 0:07212de2fec1 162 printMacAddress();
vaida 6:887df9bbe705 163
vazbyte 0:07212de2fec1 164 }
vazbyte 0:07212de2fec1 165
vazbyte 0:07212de2fec1 166 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
vazbyte 0:07212de2fec1 167 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 168 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
vazbyte 0:07212de2fec1 169 }
vazbyte 0:07212de2fec1 170
vazbyte 0:07212de2fec1 171 int main()
vazbyte 4:af0530752b76 172 {
vazbyte 2:1957a4985d6e 173 mpu.initMPU9250();
vazbyte 10:8cf4069d064a 174
vazbyte 13:99ed8a3db67a 175 eventQueue.call_every(80, blinkCallback);
vazbyte 0:07212de2fec1 176
vazbyte 0:07212de2fec1 177 BLE &ble = BLE::Instance();
vazbyte 0:07212de2fec1 178 ble.onEventsToProcess(scheduleBleEventsProcessing);
vazbyte 0:07212de2fec1 179 ble.init(bleInitComplete);
vazbyte 0:07212de2fec1 180
vazbyte 0:07212de2fec1 181 eventQueue.dispatch_forever();
vazbyte 0:07212de2fec1 182
vazbyte 0:07212de2fec1 183 return 0;
vazbyte 0:07212de2fec1 184 }