Group A / Mbed OS bucification

Pedometer

Dependencies:   MPU9250

main.cpp@2:6d3c59b866aa, 2019-01-17 (annotated)

Committer:
mynameisteodora
Date:
Thu Jan 17 23:27:17 2019 +0000
Revision:
2:6d3c59b866aa
Parent:
1:a192c8fd3da3 
Final working version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mynameisteodora 0:e1767e53fb0b 1 #include "mbed.h"
mynameisteodora 0:e1767e53fb0b 2 #include "inttypes.h"
mynameisteodora 0:e1767e53fb0b 3 #include "MPU9250.h"
mynameisteodora 0:e1767e53fb0b 4 #include "math.h"
mynameisteodora 0:e1767e53fb0b 5 #include <events/mbed_events.h>
mynameisteodora 0:e1767e53fb0b 6 #include "ble/BLE.h"
mynameisteodora 0:e1767e53fb0b 7 #include "ble/Gap.h"
mynameisteodora 0:e1767e53fb0b 8 #include "ble/services/HeartRateService.h"
mynameisteodora 2:6d3c59b866aa 9 #include "us_ticker_api.h"
mynameisteodora 0:e1767e53fb0b 10
mynameisteodora 0:e1767e53fb0b 11
mynameisteodora 0:e1767e53fb0b 12 DigitalOut led1(LED1, 1);
mynameisteodora 0:e1767e53fb0b 13
mynameisteodora 0:e1767e53fb0b 14 const static char DEVICE_NAME[] = "Pedometer";
mynameisteodora 0:e1767e53fb0b 15 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
mynameisteodora 0:e1767e53fb0b 16
mynameisteodora 0:e1767e53fb0b 17 static HeartRateService *hrServicePtr;
mynameisteodora 0:e1767e53fb0b 18
mynameisteodora 0:e1767e53fb0b 19 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
mynameisteodora 0:e1767e53fb0b 20
mynameisteodora 0:e1767e53fb0b 21 MPU9250 mpu9250(P0_26, P0_27);
mynameisteodora 0:e1767e53fb0b 22
mynameisteodora 0:e1767e53fb0b 23
mynameisteodora 0:e1767e53fb0b 24 Serial pc(USBTX, USBRX); // tx, rx
mynameisteodora 0:e1767e53fb0b 25
mynameisteodora 0:e1767e53fb0b 26 float accel_bias[3], gyro_bias[3];
mynameisteodora 0:e1767e53fb0b 27 float ax, ay, az, gx, gy, gz, mag_accel, mag_gyro;
mynameisteodora 0:e1767e53fb0b 28 short mag_accel_int = 0;
mynameisteodora 1:a192c8fd3da3 29 int og_mag_accel;
mynameisteodora 0:e1767e53fb0b 30
mynameisteodora 0:e1767e53fb0b 31 float aRes = mpu9250.aRes;
mynameisteodora 0:e1767e53fb0b 32 float gRes = mpu9250.gRes;
mynameisteodora 0:e1767e53fb0b 33
mynameisteodora 1:a192c8fd3da3 34 uint8_t step = 0;
mynameisteodora 0:e1767e53fb0b 35 uint8_t test = 0;
mynameisteodora 0:e1767e53fb0b 36
mynameisteodora 1:a192c8fd3da3 37 float x_avg, y_avg, z_avg;
mynameisteodora 2:6d3c59b866aa 38
mynameisteodora 2:6d3c59b866aa 39 // threshold should be dynamic, obtained by (max+min)/2
mynameisteodora 1:a192c8fd3da3 40 float threshold = 80.0f;
mynameisteodora 1:a192c8fd3da3 41
mynameisteodora 2:6d3c59b866aa 42 float dynamic_threshold_x = 80.0f;
mynameisteodora 2:6d3c59b866aa 43 float dynamic_threshold_y = 80.0f;
mynameisteodora 2:6d3c59b866aa 44 float dynamic_threshold_z = 80.0f;
mynameisteodora 2:6d3c59b866aa 45
mynameisteodora 2:6d3c59b866aa 46 // the first time the low_pass function is called, xm1 is 0
mynameisteodora 2:6d3c59b866aa 47 // then it will be updated with values from the previous call
mynameisteodora 2:6d3c59b866aa 48 float xm1_x = 0, xm1_y = 0, xm1_z = 0;
mynameisteodora 2:6d3c59b866aa 49 float max_lp = 120.0f;
mynameisteodora 2:6d3c59b866aa 50 float min_lp = -40.0f;
mynameisteodora 2:6d3c59b866aa 51 float threshold_lp = 80.0f;
mynameisteodora 2:6d3c59b866aa 52
mynameisteodora 2:6d3c59b866aa 53
mynameisteodora 0:e1767e53fb0b 54 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
mynameisteodora 0:e1767e53fb0b 55 {
mynameisteodora 0:e1767e53fb0b 56 BLE::Instance().gap().startAdvertising(); // restart advertising
mynameisteodora 0:e1767e53fb0b 57 }
mynameisteodora 0:e1767e53fb0b 58
mynameisteodora 2:6d3c59b866aa 59 // simple low-pass filter with two registers
mynameisteodora 2:6d3c59b866aa 60 float low_pass(float *x, float *y, int M, float xm1) {
mynameisteodora 2:6d3c59b866aa 61 int n;
mynameisteodora 2:6d3c59b866aa 62 y[0] = x[0] + xm1;
mynameisteodora 2:6d3c59b866aa 63 for(n = 1; n < M; n++) {
mynameisteodora 2:6d3c59b866aa 64 y[n] = x[n] + x[n-1];
mynameisteodora 2:6d3c59b866aa 65 }
mynameisteodora 2:6d3c59b866aa 66 return x[M-1];
mynameisteodora 2:6d3c59b866aa 67 }
mynameisteodora 2:6d3c59b866aa 68
mynameisteodora 2:6d3c59b866aa 69
mynameisteodora 0:e1767e53fb0b 70 void updateSensorValue() {
mynameisteodora 2:6d3c59b866aa 71
mynameisteodora 0:e1767e53fb0b 72 int16_t accel_data[3] = {0};
mynameisteodora 2:6d3c59b866aa 73
mynameisteodora 2:6d3c59b866aa 74 int window_size = 20;
mynameisteodora 2:6d3c59b866aa 75 int M = window_size/2;
mynameisteodora 2:6d3c59b866aa 76 // 0 - x, 1 - y, 2 - z
mynameisteodora 2:6d3c59b866aa 77 int most_active_axis = 0;
mynameisteodora 2:6d3c59b866aa 78 int flag = 0;
mynameisteodora 2:6d3c59b866aa 79
mynameisteodora 2:6d3c59b866aa 80 float in_x[window_size];
mynameisteodora 2:6d3c59b866aa 81 float in_y[window_size];
mynameisteodora 2:6d3c59b866aa 82 float in_z[window_size];
mynameisteodora 2:6d3c59b866aa 83
mynameisteodora 2:6d3c59b866aa 84 float out_x[window_size];
mynameisteodora 2:6d3c59b866aa 85 float out_y[window_size];
mynameisteodora 2:6d3c59b866aa 86 float out_z[window_size];
mynameisteodora 2:6d3c59b866aa 87
mynameisteodora 2:6d3c59b866aa 88
mynameisteodora 2:6d3c59b866aa 89 // collect n = window_size samples and detect most active axis
mynameisteodora 2:6d3c59b866aa 90 float min_x = 0.0f, min_y = 0.0f, min_z = 0.0f;
mynameisteodora 2:6d3c59b866aa 91 float max_x = 0.0f, max_y = 0.0f, max_z = 0.0f;
mynameisteodora 2:6d3c59b866aa 92 float peak_to_peak_x = 0.0f, peak_to_peak_y = 0.0f, peak_to_peak_z = 0.0f;
mynameisteodora 2:6d3c59b866aa 93
mynameisteodora 2:6d3c59b866aa 94 for(int i = 0; i < window_size; i++) {
mynameisteodora 1:a192c8fd3da3 95 mpu9250.readAccelData(accel_data);
mynameisteodora 2:6d3c59b866aa 96
mynameisteodora 2:6d3c59b866aa 97 in_x[i] = accel_data[0] - x_avg;
mynameisteodora 2:6d3c59b866aa 98 in_y[i] = accel_data[1] - y_avg;
mynameisteodora 2:6d3c59b866aa 99 in_z[i] = accel_data[2] - z_avg;
mynameisteodora 1:a192c8fd3da3 100
mynameisteodora 2:6d3c59b866aa 101 pc.printf("Accel_x: %d\n", in_x[i]);
mynameisteodora 2:6d3c59b866aa 102 pc.printf("Accel_y: %d\n", in_y[i]);
mynameisteodora 2:6d3c59b866aa 103 pc.printf("Accel_z: %d\n", in_z[i]);
mynameisteodora 2:6d3c59b866aa 104
mynameisteodora 2:6d3c59b866aa 105 if(in_x[i] > max_x) {
mynameisteodora 2:6d3c59b866aa 106 max_x = in_x[i];
mynameisteodora 2:6d3c59b866aa 107 }
mynameisteodora 2:6d3c59b866aa 108
mynameisteodora 2:6d3c59b866aa 109 if(in_y[i] > max_y) {
mynameisteodora 2:6d3c59b866aa 110 max_y = in_y[i];
mynameisteodora 2:6d3c59b866aa 111 }
mynameisteodora 2:6d3c59b866aa 112
mynameisteodora 2:6d3c59b866aa 113 if(in_z[i] > max_z) {
mynameisteodora 2:6d3c59b866aa 114 max_z = in_z[i];
mynameisteodora 2:6d3c59b866aa 115 }
mynameisteodora 2:6d3c59b866aa 116
mynameisteodora 2:6d3c59b866aa 117 if(in_x[i] < min_x) {
mynameisteodora 2:6d3c59b866aa 118 min_x = in_x[i];
mynameisteodora 1:a192c8fd3da3 119 }
mynameisteodora 1:a192c8fd3da3 120
mynameisteodora 2:6d3c59b866aa 121 if(in_y[i] < min_y) {
mynameisteodora 2:6d3c59b866aa 122 min_y = in_y[i];
mynameisteodora 2:6d3c59b866aa 123 }
mynameisteodora 2:6d3c59b866aa 124
mynameisteodora 2:6d3c59b866aa 125 if(in_z[i] < min_z) {
mynameisteodora 2:6d3c59b866aa 126 min_z = in_z[i];
mynameisteodora 1:a192c8fd3da3 127 }
mynameisteodora 2:6d3c59b866aa 128 }
mynameisteodora 1:a192c8fd3da3 129
mynameisteodora 2:6d3c59b866aa 130 peak_to_peak_x = max_x - min_x;
mynameisteodora 2:6d3c59b866aa 131 peak_to_peak_y = max_y - min_y;
mynameisteodora 2:6d3c59b866aa 132 peak_to_peak_z = max_z - max_z;
mynameisteodora 2:6d3c59b866aa 133
mynameisteodora 2:6d3c59b866aa 134 if(peak_to_peak_x > peak_to_peak_y) {
mynameisteodora 2:6d3c59b866aa 135 if(peak_to_peak_x > peak_to_peak_z) {
mynameisteodora 2:6d3c59b866aa 136 dynamic_threshold_x = (min_x + max_x)/2;
mynameisteodora 2:6d3c59b866aa 137 most_active_axis = 0;
mynameisteodora 2:6d3c59b866aa 138 pc.printf("Most active axis: X\n");
mynameisteodora 2:6d3c59b866aa 139 }
mynameisteodora 2:6d3c59b866aa 140 else {
mynameisteodora 2:6d3c59b866aa 141 dynamic_threshold_z = (min_z + max_z)/2;
mynameisteodora 2:6d3c59b866aa 142 most_active_axis = 2;
mynameisteodora 2:6d3c59b866aa 143 pc.printf("Most active axis: Z\n");
mynameisteodora 0:e1767e53fb0b 144 }
mynameisteodora 1:a192c8fd3da3 145 }
mynameisteodora 2:6d3c59b866aa 146 else if(peak_to_peak_y > peak_to_peak_z) {
mynameisteodora 2:6d3c59b866aa 147 dynamic_threshold_y = (min_y + max_y)/2;
mynameisteodora 2:6d3c59b866aa 148 most_active_axis = 1;
mynameisteodora 2:6d3c59b866aa 149 pc.printf("Most active axis: Y\n");
mynameisteodora 2:6d3c59b866aa 150 }
mynameisteodora 2:6d3c59b866aa 151 else {
mynameisteodora 2:6d3c59b866aa 152 dynamic_threshold_z = (min_z + max_z)/2;
mynameisteodora 2:6d3c59b866aa 153 most_active_axis = 2;
mynameisteodora 2:6d3c59b866aa 154 pc.printf("Most active axis: Z\n");
mynameisteodora 2:6d3c59b866aa 155 }
mynameisteodora 2:6d3c59b866aa 156
mynameisteodora 2:6d3c59b866aa 157 if(most_active_axis == 0) {
mynameisteodora 2:6d3c59b866aa 158 // low pass on x-axis
mynameisteodora 2:6d3c59b866aa 159 xm1_x = low_pass(in_x, out_x, M, xm1_x);
mynameisteodora 2:6d3c59b866aa 160 xm1_x = low_pass(&in_x[M], &out_x[M], M, xm1_x);
mynameisteodora 2:6d3c59b866aa 161 pc.printf("Low passed axis X\n");
mynameisteodora 2:6d3c59b866aa 162
mynameisteodora 2:6d3c59b866aa 163 // now analyse the output data, out_x, to see if the threshold has been passed
mynameisteodora 2:6d3c59b866aa 164 for(int i = 1; i < window_size; i++) {
mynameisteodora 2:6d3c59b866aa 165
mynameisteodora 2:6d3c59b866aa 166 // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
mynameisteodora 2:6d3c59b866aa 167 if(out_x[i] < out_x[i-1] && out_x[i] < dynamic_threshold_x && out_x[i-1] > dynamic_threshold_x && flag == 0) {
mynameisteodora 2:6d3c59b866aa 168
mynameisteodora 2:6d3c59b866aa 169 step = 1;
mynameisteodora 2:6d3c59b866aa 170 flag = 1;
mynameisteodora 2:6d3c59b866aa 171 pc.printf("Step!\n");
mynameisteodora 2:6d3c59b866aa 172 hrServicePtr->updateHeartRate(step);
mynameisteodora 2:6d3c59b866aa 173 wait(0.50);
mynameisteodora 2:6d3c59b866aa 174
mynameisteodora 2:6d3c59b866aa 175 }
mynameisteodora 2:6d3c59b866aa 176
mynameisteodora 2:6d3c59b866aa 177 else if (out_x[i] < out_x[i-1] && out_x[i] < dynamic_threshold_x && out_x[i-1] > dynamic_threshold_x && flag == 1) {
mynameisteodora 2:6d3c59b866aa 178 // do nothing
mynameisteodora 2:6d3c59b866aa 179 }
mynameisteodora 2:6d3c59b866aa 180
mynameisteodora 2:6d3c59b866aa 181 // if the threshold is being crossed from the lower half to the upper half and the flag is set to 1
mynameisteodora 2:6d3c59b866aa 182 else if (out_x[i] > out_x[i-1] && out_x[i] > dynamic_threshold_x && out_x[i-1] < dynamic_threshold_x && flag == 1) {
mynameisteodora 2:6d3c59b866aa 183 // this is a step but we are counting gaits
mynameisteodora 2:6d3c59b866aa 184 // however, we need to set the flag to 0
mynameisteodora 2:6d3c59b866aa 185 flag = 0;
mynameisteodora 2:6d3c59b866aa 186 }
mynameisteodora 2:6d3c59b866aa 187 }
mynameisteodora 2:6d3c59b866aa 188 }
mynameisteodora 2:6d3c59b866aa 189
mynameisteodora 2:6d3c59b866aa 190 else if(most_active_axis == 1) {
mynameisteodora 2:6d3c59b866aa 191 // low pass on y-axis
mynameisteodora 2:6d3c59b866aa 192 xm1_y = low_pass(in_y, out_y, M, xm1_y);
mynameisteodora 2:6d3c59b866aa 193 xm1_y = low_pass(&in_y[M], &out_y[M], M, xm1_y);
mynameisteodora 2:6d3c59b866aa 194 pc.printf("Low passed axis Y\n");
mynameisteodora 0:e1767e53fb0b 195
mynameisteodora 2:6d3c59b866aa 196 // now analyse the output data, out_y, to see if the threshold has been passed
mynameisteodora 2:6d3c59b866aa 197 for(int i = 1; i < window_size; i++) {
mynameisteodora 2:6d3c59b866aa 198
mynameisteodora 2:6d3c59b866aa 199 // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
mynameisteodora 2:6d3c59b866aa 200 if(out_y[i] < out_y[i-1] && out_y[i] < dynamic_threshold_y && out_y[i-1] > dynamic_threshold_y && flag == 0) {
mynameisteodora 2:6d3c59b866aa 201
mynameisteodora 2:6d3c59b866aa 202 step = 1;
mynameisteodora 2:6d3c59b866aa 203 flag = 1;
mynameisteodora 2:6d3c59b866aa 204 pc.printf("Step!\n");
mynameisteodora 2:6d3c59b866aa 205 hrServicePtr->updateHeartRate(step);
mynameisteodora 2:6d3c59b866aa 206 wait(0.50);
mynameisteodora 2:6d3c59b866aa 207
mynameisteodora 2:6d3c59b866aa 208 }
mynameisteodora 2:6d3c59b866aa 209
mynameisteodora 2:6d3c59b866aa 210 else if (out_y[i] < out_y[i-1] && out_y[i] < dynamic_threshold_y && out_y[i-1] > dynamic_threshold_y && flag == 1) {
mynameisteodora 2:6d3c59b866aa 211 // do nothing
mynameisteodora 2:6d3c59b866aa 212 }
mynameisteodora 2:6d3c59b866aa 213
mynameisteodora 2:6d3c59b866aa 214 // if the threshold is being crossed from the lower half to the upper half and the flag is set to 1
mynameisteodora 2:6d3c59b866aa 215 else if (out_y[i] > out_y[i-1] && out_y[i] > dynamic_threshold_y && out_y[i-1] < dynamic_threshold_y && flag == 1) {
mynameisteodora 2:6d3c59b866aa 216 // this is a step but we are counting gaits
mynameisteodora 2:6d3c59b866aa 217 // however, we need to set the flag to 0
mynameisteodora 2:6d3c59b866aa 218 flag = 0;
mynameisteodora 2:6d3c59b866aa 219 }
mynameisteodora 2:6d3c59b866aa 220 }
mynameisteodora 2:6d3c59b866aa 221 }
mynameisteodora 2:6d3c59b866aa 222 else if(most_active_axis == 2) {
mynameisteodora 2:6d3c59b866aa 223 // low pass on z-axis
mynameisteodora 2:6d3c59b866aa 224 xm1_z = low_pass(in_z, out_z, M, xm1_z);
mynameisteodora 2:6d3c59b866aa 225 xm1_z = low_pass(&in_z[M], &out_z[M], M, xm1_z);
mynameisteodora 2:6d3c59b866aa 226
mynameisteodora 2:6d3c59b866aa 227 pc.printf("Low passed axis Z\n");
mynameisteodora 2:6d3c59b866aa 228
mynameisteodora 2:6d3c59b866aa 229 // now analyse the output data, out_z, to see if the threshold has been passed
mynameisteodora 2:6d3c59b866aa 230 for(int i = 1; i < window_size; i++) {
mynameisteodora 2:6d3c59b866aa 231
mynameisteodora 2:6d3c59b866aa 232 // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
mynameisteodora 2:6d3c59b866aa 233 if(out_z[i] < out_z[i-1] && out_z[i] < dynamic_threshold_z && out_z[i-1] > dynamic_threshold_z && flag == 0) {
mynameisteodora 2:6d3c59b866aa 234
mynameisteodora 2:6d3c59b866aa 235 step = 1;
mynameisteodora 2:6d3c59b866aa 236 flag = 1;
mynameisteodora 2:6d3c59b866aa 237 pc.printf("Step!\n");
mynameisteodora 2:6d3c59b866aa 238 hrServicePtr->updateHeartRate(step);
mynameisteodora 2:6d3c59b866aa 239 wait(0.50);
mynameisteodora 2:6d3c59b866aa 240 }
mynameisteodora 2:6d3c59b866aa 241
mynameisteodora 2:6d3c59b866aa 242
mynameisteodora 2:6d3c59b866aa 243
mynameisteodora 2:6d3c59b866aa 244 else if (out_z[i] < out_z[i-1] && out_z[i] < dynamic_threshold_z && out_z[i-1] > dynamic_threshold_z && flag == 1) {
mynameisteodora 2:6d3c59b866aa 245 // do nothing
mynameisteodora 2:6d3c59b866aa 246 }
mynameisteodora 2:6d3c59b866aa 247
mynameisteodora 2:6d3c59b866aa 248 else if (out_z[i] > out_z[i-1] && out_z[i] > dynamic_threshold_z && out_z[i-1] < dynamic_threshold_z && flag == 1) {
mynameisteodora 2:6d3c59b866aa 249 // this is a step but we are counting gaits
mynameisteodora 2:6d3c59b866aa 250 // however, we need to set the flag to 0
mynameisteodora 2:6d3c59b866aa 251 flag = 0;
mynameisteodora 2:6d3c59b866aa 252 }
mynameisteodora 2:6d3c59b866aa 253 }
mynameisteodora 2:6d3c59b866aa 254 }
mynameisteodora 2:6d3c59b866aa 255 }
mynameisteodora 1:a192c8fd3da3 256
mynameisteodora 0:e1767e53fb0b 257
mynameisteodora 0:e1767e53fb0b 258 void periodicCallback(void)
mynameisteodora 0:e1767e53fb0b 259 {
mynameisteodora 0:e1767e53fb0b 260 led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
mynameisteodora 0:e1767e53fb0b 261
mynameisteodora 0:e1767e53fb0b 262 if (BLE::Instance().getGapState().connected) {
mynameisteodora 0:e1767e53fb0b 263 eventQueue.call(updateSensorValue);
mynameisteodora 0:e1767e53fb0b 264 }
mynameisteodora 0:e1767e53fb0b 265 }
mynameisteodora 0:e1767e53fb0b 266
mynameisteodora 0:e1767e53fb0b 267 void onBleInitError(BLE &ble, ble_error_t error)
mynameisteodora 0:e1767e53fb0b 268 {
mynameisteodora 0:e1767e53fb0b 269 (void)ble;
mynameisteodora 0:e1767e53fb0b 270 (void)error;
mynameisteodora 0:e1767e53fb0b 271 /* Initialization error handling should go here */
mynameisteodora 0:e1767e53fb0b 272 }
mynameisteodora 0:e1767e53fb0b 273
mynameisteodora 0:e1767e53fb0b 274 void printMacAddress()
mynameisteodora 0:e1767e53fb0b 275 {
mynameisteodora 0:e1767e53fb0b 276 /* Print out device MAC address to the console*/
mynameisteodora 0:e1767e53fb0b 277 Gap::AddressType_t addr_type;
mynameisteodora 0:e1767e53fb0b 278 Gap::Address_t address;
mynameisteodora 0:e1767e53fb0b 279 BLE::Instance().gap().getAddress(&addr_type, address);
mynameisteodora 0:e1767e53fb0b 280 printf("DEVICE MAC ADDRESS: ");
mynameisteodora 0:e1767e53fb0b 281 for (int i = 5; i >= 1; i--){
mynameisteodora 0:e1767e53fb0b 282 printf("%02x:", address[i]);
mynameisteodora 0:e1767e53fb0b 283 }
mynameisteodora 0:e1767e53fb0b 284 printf("%02x\r\n", address[0]);
mynameisteodora 0:e1767e53fb0b 285 }
mynameisteodora 0:e1767e53fb0b 286
mynameisteodora 0:e1767e53fb0b 287 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
mynameisteodora 0:e1767e53fb0b 288 {
mynameisteodora 0:e1767e53fb0b 289 BLE& ble = params->ble;
mynameisteodora 0:e1767e53fb0b 290 ble_error_t error = params->error;
mynameisteodora 0:e1767e53fb0b 291
mynameisteodora 0:e1767e53fb0b 292 if (error != BLE_ERROR_NONE) {
mynameisteodora 0:e1767e53fb0b 293 onBleInitError(ble, error);
mynameisteodora 0:e1767e53fb0b 294 return;
mynameisteodora 0:e1767e53fb0b 295 }
mynameisteodora 0:e1767e53fb0b 296
mynameisteodora 0:e1767e53fb0b 297 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
mynameisteodora 0:e1767e53fb0b 298 return;
mynameisteodora 0:e1767e53fb0b 299 }
mynameisteodora 0:e1767e53fb0b 300
mynameisteodora 0:e1767e53fb0b 301 ble.gap().onDisconnection(disconnectionCallback);
mynameisteodora 0:e1767e53fb0b 302
mynameisteodora 0:e1767e53fb0b 303 /* Setup primary service. */
mynameisteodora 0:e1767e53fb0b 304 hrServicePtr = new HeartRateService(ble, mag_accel_int, HeartRateService::LOCATION_FINGER);
mynameisteodora 0:e1767e53fb0b 305
mynameisteodora 0:e1767e53fb0b 306 /* Setup advertising. */
mynameisteodora 0:e1767e53fb0b 307 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mynameisteodora 0:e1767e53fb0b 308 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
mynameisteodora 0:e1767e53fb0b 309 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
mynameisteodora 0:e1767e53fb0b 310 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mynameisteodora 0:e1767e53fb0b 311 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mynameisteodora 0:e1767e53fb0b 312 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
mynameisteodora 0:e1767e53fb0b 313 ble.gap().startAdvertising();
mynameisteodora 0:e1767e53fb0b 314
mynameisteodora 0:e1767e53fb0b 315 printMacAddress();
mynameisteodora 0:e1767e53fb0b 316 }
mynameisteodora 0:e1767e53fb0b 317
mynameisteodora 0:e1767e53fb0b 318 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
mynameisteodora 0:e1767e53fb0b 319 BLE &ble = BLE::Instance();
mynameisteodora 0:e1767e53fb0b 320 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
mynameisteodora 0:e1767e53fb0b 321 }
mynameisteodora 0:e1767e53fb0b 322
mynameisteodora 0:e1767e53fb0b 323 int main() {
mynameisteodora 0:e1767e53fb0b 324 pc.baud(9600);
mynameisteodora 0:e1767e53fb0b 325
mynameisteodora 0:e1767e53fb0b 326 mpu9250.resetMPU9250();
mynameisteodora 0:e1767e53fb0b 327 pc.printf("MPU reset\n");
mynameisteodora 0:e1767e53fb0b 328
mynameisteodora 0:e1767e53fb0b 329 mpu9250.calibrateMPU9250(accel_bias, gyro_bias);
mynameisteodora 1:a192c8fd3da3 330 pc.printf("Library calibration done!\n");
mynameisteodora 1:a192c8fd3da3 331
mynameisteodora 2:6d3c59b866aa 332 // Implement own calibration to estimate the threshold values
mynameisteodora 1:a192c8fd3da3 333 int sum_x = 0, sum_y = 0, sum_z = 0;
mynameisteodora 1:a192c8fd3da3 334 int xvals[100] = {0}, yvals[100] = {0}, zvals[100] = {0};
mynameisteodora 1:a192c8fd3da3 335 int16_t local_accel_data[3] = {0};
mynameisteodora 1:a192c8fd3da3 336
mynameisteodora 0:e1767e53fb0b 337
mynameisteodora 1:a192c8fd3da3 338 for(int i = 0; i < 100; i++) {
mynameisteodora 1:a192c8fd3da3 339 mpu9250.readAccelData(local_accel_data);
mynameisteodora 1:a192c8fd3da3 340 xvals[i] = local_accel_data[0];
mynameisteodora 1:a192c8fd3da3 341 yvals[i] = local_accel_data[1];
mynameisteodora 1:a192c8fd3da3 342 zvals[i] = local_accel_data[2];
mynameisteodora 1:a192c8fd3da3 343 sum_x += xvals[i];
mynameisteodora 1:a192c8fd3da3 344 sum_y += yvals[i];
mynameisteodora 1:a192c8fd3da3 345 sum_z += zvals[i];
mynameisteodora 1:a192c8fd3da3 346 }
mynameisteodora 1:a192c8fd3da3 347
mynameisteodora 1:a192c8fd3da3 348 x_avg = sum_x/100.0f;
mynameisteodora 1:a192c8fd3da3 349 y_avg = sum_y/100.0f;
mynameisteodora 1:a192c8fd3da3 350 z_avg = sum_z/100.0f;
mynameisteodora 1:a192c8fd3da3 351
mynameisteodora 1:a192c8fd3da3 352 pc.printf("Accel_x average: %f\n", x_avg);
mynameisteodora 1:a192c8fd3da3 353 pc.printf("Accel_y average: %f\n", y_avg);
mynameisteodora 1:a192c8fd3da3 354 pc.printf("Accel_z average: %f\n", z_avg);
mynameisteodora 1:a192c8fd3da3 355
mynameisteodora 1:a192c8fd3da3 356 pc.printf("Accel bias: %f\n", accel_bias[0]);
mynameisteodora 0:e1767e53fb0b 357
mynameisteodora 0:e1767e53fb0b 358 mpu9250.initMPU9250();
mynameisteodora 0:e1767e53fb0b 359 pc.printf("Initialisation successful!\n");
mynameisteodora 0:e1767e53fb0b 360
mynameisteodora 0:e1767e53fb0b 361 mpu9250.getAres();
mynameisteodora 0:e1767e53fb0b 362 pc.printf("Accel sensitivity: %f\n", aRes);
mynameisteodora 0:e1767e53fb0b 363
mynameisteodora 2:6d3c59b866aa 364 // the sensor readings are updated every second
mynameisteodora 1:a192c8fd3da3 365 eventQueue.call_every(1000, periodicCallback);
mynameisteodora 0:e1767e53fb0b 366
mynameisteodora 0:e1767e53fb0b 367 BLE &ble = BLE::Instance();
mynameisteodora 0:e1767e53fb0b 368 pc.printf("BLE instance created!\n");
mynameisteodora 0:e1767e53fb0b 369 ble.onEventsToProcess(scheduleBleEventsProcessing);
mynameisteodora 0:e1767e53fb0b 370 pc.printf("BLE events scheduled!\n");
mynameisteodora 0:e1767e53fb0b 371 ble.init(bleInitComplete);
mynameisteodora 0:e1767e53fb0b 372 pc.printf("BLE init complete!\n");
mynameisteodora 0:e1767e53fb0b 373
mynameisteodora 0:e1767e53fb0b 374 eventQueue.dispatch_forever();
mynameisteodora 0:e1767e53fb0b 375 pc.printf("Dispatched");
mynameisteodora 0:e1767e53fb0b 376
mynameisteodora 0:e1767e53fb0b 377 return 0;
mynameisteodora 0:e1767e53fb0b 378
mynameisteodora 0:e1767e53fb0b 379
mynameisteodora 0:e1767e53fb0b 380 }
mynameisteodora 0:e1767e53fb0b 381
mynameisteodora 0:e1767e53fb0b 382
mynameisteodora 0:e1767e53fb0b 383