Group A / Mbed OS bucification

Pedometer

Dependencies:   MPU9250

main.cpp@1:a192c8fd3da3, 2018-11-13 (annotated)

Committer:
mynameisteodora
Date:
Tue Nov 13 23:52:30 2018 +0000
Revision:
1:a192c8fd3da3
Parent:
0:e1767e53fb0b
Child:
2:6d3c59b866aa
rough step counting

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 0:e1767e53fb0b 9
mynameisteodora 0:e1767e53fb0b 10
mynameisteodora 0:e1767e53fb0b 11 DigitalOut led1(LED1, 1);
mynameisteodora 0:e1767e53fb0b 12
mynameisteodora 0:e1767e53fb0b 13 const static char DEVICE_NAME[] = "Pedometer";
mynameisteodora 0:e1767e53fb0b 14 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
mynameisteodora 0:e1767e53fb0b 15
mynameisteodora 0:e1767e53fb0b 16 //static uint8_t hrmCounter = 100; // init HRM to 100bps
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 1:a192c8fd3da3 38 float threshold = 80.0f;
mynameisteodora 1:a192c8fd3da3 39
mynameisteodora 0:e1767e53fb0b 40 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
mynameisteodora 0:e1767e53fb0b 41 {
mynameisteodora 0:e1767e53fb0b 42 BLE::Instance().gap().startAdvertising(); // restart advertising
mynameisteodora 0:e1767e53fb0b 43 }
mynameisteodora 0:e1767e53fb0b 44
mynameisteodora 0:e1767e53fb0b 45 void updateSensorValue() {
mynameisteodora 0:e1767e53fb0b 46 // Do blocking calls or whatever is necessary for sensor polling.
mynameisteodora 0:e1767e53fb0b 47 // In our case, we simply update the HRM measurement.
mynameisteodora 0:e1767e53fb0b 48 int16_t accel_data[3] = {0};
mynameisteodora 0:e1767e53fb0b 49 int16_t gyro_data[3] = {0};
mynameisteodora 1:a192c8fd3da3 50 float ax, ay, az;
mynameisteodora 1:a192c8fd3da3 51 float magnitude[100] = {0};
mynameisteodora 1:a192c8fd3da3 52 float rolling_average[100] = {0};
mynameisteodora 1:a192c8fd3da3 53 short flag = 0;
mynameisteodora 1:a192c8fd3da3 54
mynameisteodora 1:a192c8fd3da3 55 pc.printf("Accel_x average: %f\n", x_avg);
mynameisteodora 1:a192c8fd3da3 56 pc.printf("Accel_y average: %f\n", y_avg);
mynameisteodora 1:a192c8fd3da3 57 pc.printf("Accel_z average: %f\n", z_avg);
mynameisteodora 1:a192c8fd3da3 58
mynameisteodora 0:e1767e53fb0b 59 mpu9250.readAccelData(accel_data);
mynameisteodora 0:e1767e53fb0b 60 pc.printf("Accel_x: %d\n", accel_data[0]);
mynameisteodora 1:a192c8fd3da3 61 pc.printf("Accel_y: %d\n", accel_data[1]);
mynameisteodora 1:a192c8fd3da3 62 pc.printf("Accel_z: %d\n", accel_data[2]);
mynameisteodora 1:a192c8fd3da3 63
mynameisteodora 0:e1767e53fb0b 64 mpu9250.readGyroData(gyro_data);
mynameisteodora 0:e1767e53fb0b 65 pc.printf("Gyro_x: %d\n", gyro_data[0]);
mynameisteodora 1:a192c8fd3da3 66 pc.printf("Gyro_y: %d\n", gyro_data[1]);
mynameisteodora 1:a192c8fd3da3 67 pc.printf("Gyro_z: %d\n", gyro_data[2]);
mynameisteodora 1:a192c8fd3da3 68
mynameisteodora 1:a192c8fd3da3 69 ax = accel_data[0] - x_avg;
mynameisteodora 1:a192c8fd3da3 70 ay = accel_data[1] - y_avg;
mynameisteodora 1:a192c8fd3da3 71 az = accel_data[2] - z_avg;
mynameisteodora 1:a192c8fd3da3 72
mynameisteodora 1:a192c8fd3da3 73 magnitude[0] = sqrt(ax*ax + ay*ay + az*az);
mynameisteodora 1:a192c8fd3da3 74
mynameisteodora 1:a192c8fd3da3 75 for(int i = 1; i < 100; i++) {
mynameisteodora 1:a192c8fd3da3 76 step = 0;
mynameisteodora 1:a192c8fd3da3 77 mpu9250.readAccelData(accel_data);
mynameisteodora 1:a192c8fd3da3 78 pc.printf("Accel_x: %d\n", accel_data[0]);
mynameisteodora 1:a192c8fd3da3 79 pc.printf("Accel_y: %d\n", accel_data[1]);
mynameisteodora 1:a192c8fd3da3 80 pc.printf("Accel_z: %d\n", accel_data[2]);
mynameisteodora 1:a192c8fd3da3 81
mynameisteodora 1:a192c8fd3da3 82 mpu9250.readGyroData(gyro_data);
mynameisteodora 1:a192c8fd3da3 83 pc.printf("Gyro_x: %d\n", gyro_data[0]);
mynameisteodora 1:a192c8fd3da3 84 pc.printf("Gyro_y: %d\n", gyro_data[1]);
mynameisteodora 1:a192c8fd3da3 85 pc.printf("Gyro_z: %d\n", gyro_data[2]);
mynameisteodora 1:a192c8fd3da3 86
mynameisteodora 1:a192c8fd3da3 87 ax = accel_data[0] - x_avg;
mynameisteodora 1:a192c8fd3da3 88 ay = accel_data[1] - y_avg;
mynameisteodora 1:a192c8fd3da3 89 az = accel_data[2] - z_avg;
mynameisteodora 1:a192c8fd3da3 90
mynameisteodora 1:a192c8fd3da3 91 magnitude[i] = sqrt(ax*ax + ay*ay + az*az);
mynameisteodora 1:a192c8fd3da3 92 rolling_average[i] = (magnitude[i] - magnitude[i-1])/2;
mynameisteodora 1:a192c8fd3da3 93 pc.printf("Rolling average: %f\n", rolling_average[i]);
mynameisteodora 0:e1767e53fb0b 94
mynameisteodora 1:a192c8fd3da3 95 if (rolling_average[i]>threshold && flag==0) {
mynameisteodora 1:a192c8fd3da3 96 // a step has been taken
mynameisteodora 1:a192c8fd3da3 97 step = 1;
mynameisteodora 1:a192c8fd3da3 98 hrServicePtr->updateHeartRate(step);
mynameisteodora 1:a192c8fd3da3 99 pc.printf("Sensor value updated with value: %d\n", step);
mynameisteodora 1:a192c8fd3da3 100 flag=1;
mynameisteodora 1:a192c8fd3da3 101 }
mynameisteodora 1:a192c8fd3da3 102
mynameisteodora 1:a192c8fd3da3 103 else if (rolling_average[i] > threshold && flag==1) {
mynameisteodora 1:a192c8fd3da3 104
mynameisteodora 1:a192c8fd3da3 105 //do nothing
mynameisteodora 1:a192c8fd3da3 106 }
mynameisteodora 1:a192c8fd3da3 107
mynameisteodora 1:a192c8fd3da3 108 if (rolling_average[i] <threshold && flag==1) {
mynameisteodora 1:a192c8fd3da3 109 flag=0;
mynameisteodora 0:e1767e53fb0b 110 }
mynameisteodora 1:a192c8fd3da3 111 }
mynameisteodora 1:a192c8fd3da3 112 // mpu9250.readAccelData(accel_data);
mynameisteodora 1:a192c8fd3da3 113 // pc.printf("Accel_x: %d\n", accel_data[0]);
mynameisteodora 1:a192c8fd3da3 114 // pc.printf("Accel_y: %d\n", accel_data[1]);
mynameisteodora 1:a192c8fd3da3 115 // pc.printf("Accel_z: %d\n", accel_data[2]);
mynameisteodora 1:a192c8fd3da3 116 //
mynameisteodora 1:a192c8fd3da3 117 // mpu9250.readGyroData(gyro_data);
mynameisteodora 1:a192c8fd3da3 118 // pc.printf("Gyro_x: %d\n", gyro_data[0]);
mynameisteodora 1:a192c8fd3da3 119 // pc.printf("Gyro_y: %d\n", gyro_data[1]);
mynameisteodora 1:a192c8fd3da3 120 // pc.printf("Gyro_z: %d\n", gyro_data[2]);
mynameisteodora 1:a192c8fd3da3 121 //
mynameisteodora 1:a192c8fd3da3 122 // og_mag_accel = accel_data[0]*accel_data[0] + accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2];
mynameisteodora 1:a192c8fd3da3 123 // pc.printf("Magnitude of accel: %d\n", og_mag_accel);
mynameisteodora 1:a192c8fd3da3 124 //
mynameisteodora 1:a192c8fd3da3 125 // pc.printf("Accel Res: %f\n", aRes);
mynameisteodora 1:a192c8fd3da3 126 //
mynameisteodora 1:a192c8fd3da3 127 // threshold = (0.15f * 0.15f)*16384*16384;
mynameisteodora 1:a192c8fd3da3 128 // pc.printf("Threshold: %f\n", threshold);
mynameisteodora 1:a192c8fd3da3 129 //
mynameisteodora 1:a192c8fd3da3 130 // // If the acceleration vector is greater than 0.15, add the steps
mynameisteodora 1:a192c8fd3da3 131 // if(og_mag_accel > (int) threshold) {
mynameisteodora 1:a192c8fd3da3 132 // if(test > 254) {
mynameisteodora 1:a192c8fd3da3 133 // test = 0;
mynameisteodora 1:a192c8fd3da3 134 // }
mynameisteodora 1:a192c8fd3da3 135 // test++;
mynameisteodora 1:a192c8fd3da3 136 // step = step + 2;
mynameisteodora 1:a192c8fd3da3 137 // }
mynameisteodora 1:a192c8fd3da3 138 //
mynameisteodora 1:a192c8fd3da3 139 // // Avoiding counting the same steps twice
mynameisteodora 1:a192c8fd3da3 140 // //wait(0.65);
mynameisteodora 1:a192c8fd3da3 141 //
mynameisteodora 0:e1767e53fb0b 142
mynameisteodora 1:a192c8fd3da3 143
mynameisteodora 1:a192c8fd3da3 144 //
mynameisteodora 1:a192c8fd3da3 145 // step++;
mynameisteodora 1:a192c8fd3da3 146 //
mynameisteodora 1:a192c8fd3da3 147 // hrServicePtr->updateHeartRate(step);
mynameisteodora 1:a192c8fd3da3 148 // pc.printf("Sensor value updated AGAIN with value: %d\n", step);
mynameisteodora 0:e1767e53fb0b 149 }
mynameisteodora 0:e1767e53fb0b 150
mynameisteodora 0:e1767e53fb0b 151 void periodicCallback(void)
mynameisteodora 0:e1767e53fb0b 152 {
mynameisteodora 0:e1767e53fb0b 153 led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
mynameisteodora 0:e1767e53fb0b 154
mynameisteodora 0:e1767e53fb0b 155 if (BLE::Instance().getGapState().connected) {
mynameisteodora 0:e1767e53fb0b 156 eventQueue.call(updateSensorValue);
mynameisteodora 0:e1767e53fb0b 157 }
mynameisteodora 0:e1767e53fb0b 158 }
mynameisteodora 0:e1767e53fb0b 159
mynameisteodora 0:e1767e53fb0b 160 void onBleInitError(BLE &ble, ble_error_t error)
mynameisteodora 0:e1767e53fb0b 161 {
mynameisteodora 0:e1767e53fb0b 162 (void)ble;
mynameisteodora 0:e1767e53fb0b 163 (void)error;
mynameisteodora 0:e1767e53fb0b 164 /* Initialization error handling should go here */
mynameisteodora 0:e1767e53fb0b 165 }
mynameisteodora 0:e1767e53fb0b 166
mynameisteodora 0:e1767e53fb0b 167 void printMacAddress()
mynameisteodora 0:e1767e53fb0b 168 {
mynameisteodora 0:e1767e53fb0b 169 /* Print out device MAC address to the console*/
mynameisteodora 0:e1767e53fb0b 170 Gap::AddressType_t addr_type;
mynameisteodora 0:e1767e53fb0b 171 Gap::Address_t address;
mynameisteodora 0:e1767e53fb0b 172 BLE::Instance().gap().getAddress(&addr_type, address);
mynameisteodora 0:e1767e53fb0b 173 printf("DEVICE MAC ADDRESS: ");
mynameisteodora 0:e1767e53fb0b 174 for (int i = 5; i >= 1; i--){
mynameisteodora 0:e1767e53fb0b 175 printf("%02x:", address[i]);
mynameisteodora 0:e1767e53fb0b 176 }
mynameisteodora 0:e1767e53fb0b 177 printf("%02x\r\n", address[0]);
mynameisteodora 0:e1767e53fb0b 178 }
mynameisteodora 0:e1767e53fb0b 179
mynameisteodora 0:e1767e53fb0b 180 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
mynameisteodora 0:e1767e53fb0b 181 {
mynameisteodora 0:e1767e53fb0b 182 BLE& ble = params->ble;
mynameisteodora 0:e1767e53fb0b 183 ble_error_t error = params->error;
mynameisteodora 0:e1767e53fb0b 184
mynameisteodora 0:e1767e53fb0b 185 if (error != BLE_ERROR_NONE) {
mynameisteodora 0:e1767e53fb0b 186 onBleInitError(ble, error);
mynameisteodora 0:e1767e53fb0b 187 return;
mynameisteodora 0:e1767e53fb0b 188 }
mynameisteodora 0:e1767e53fb0b 189
mynameisteodora 0:e1767e53fb0b 190 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
mynameisteodora 0:e1767e53fb0b 191 return;
mynameisteodora 0:e1767e53fb0b 192 }
mynameisteodora 0:e1767e53fb0b 193
mynameisteodora 0:e1767e53fb0b 194 ble.gap().onDisconnection(disconnectionCallback);
mynameisteodora 0:e1767e53fb0b 195
mynameisteodora 0:e1767e53fb0b 196 /* Setup primary service. */
mynameisteodora 0:e1767e53fb0b 197 hrServicePtr = new HeartRateService(ble, mag_accel_int, HeartRateService::LOCATION_FINGER);
mynameisteodora 0:e1767e53fb0b 198
mynameisteodora 0:e1767e53fb0b 199 /* Setup advertising. */
mynameisteodora 0:e1767e53fb0b 200 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
mynameisteodora 0:e1767e53fb0b 201 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
mynameisteodora 0:e1767e53fb0b 202 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
mynameisteodora 0:e1767e53fb0b 203 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
mynameisteodora 0:e1767e53fb0b 204 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
mynameisteodora 0:e1767e53fb0b 205 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
mynameisteodora 0:e1767e53fb0b 206 ble.gap().startAdvertising();
mynameisteodora 0:e1767e53fb0b 207
mynameisteodora 0:e1767e53fb0b 208 printMacAddress();
mynameisteodora 0:e1767e53fb0b 209 }
mynameisteodora 0:e1767e53fb0b 210
mynameisteodora 0:e1767e53fb0b 211 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
mynameisteodora 0:e1767e53fb0b 212 BLE &ble = BLE::Instance();
mynameisteodora 0:e1767e53fb0b 213 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
mynameisteodora 0:e1767e53fb0b 214 }
mynameisteodora 0:e1767e53fb0b 215
mynameisteodora 0:e1767e53fb0b 216 int main() {
mynameisteodora 0:e1767e53fb0b 217 pc.baud(9600);
mynameisteodora 0:e1767e53fb0b 218 pc.printf("Hello World!\n");
mynameisteodora 0:e1767e53fb0b 219
mynameisteodora 0:e1767e53fb0b 220 mpu9250.resetMPU9250();
mynameisteodora 0:e1767e53fb0b 221 pc.printf("MPU reset\n");
mynameisteodora 0:e1767e53fb0b 222
mynameisteodora 0:e1767e53fb0b 223 mpu9250.calibrateMPU9250(accel_bias, gyro_bias);
mynameisteodora 1:a192c8fd3da3 224 pc.printf("Library calibration done!\n");
mynameisteodora 1:a192c8fd3da3 225
mynameisteodora 1:a192c8fd3da3 226 // try "calibrating" to get the threshold
mynameisteodora 1:a192c8fd3da3 227 int sum_x = 0, sum_y = 0, sum_z = 0;
mynameisteodora 1:a192c8fd3da3 228 int xvals[100] = {0}, yvals[100] = {0}, zvals[100] = {0};
mynameisteodora 1:a192c8fd3da3 229 int16_t local_accel_data[3] = {0};
mynameisteodora 1:a192c8fd3da3 230
mynameisteodora 0:e1767e53fb0b 231
mynameisteodora 1:a192c8fd3da3 232 for(int i = 0; i < 100; i++) {
mynameisteodora 1:a192c8fd3da3 233 mpu9250.readAccelData(local_accel_data);
mynameisteodora 1:a192c8fd3da3 234 xvals[i] = local_accel_data[0];
mynameisteodora 1:a192c8fd3da3 235 yvals[i] = local_accel_data[1];
mynameisteodora 1:a192c8fd3da3 236 zvals[i] = local_accel_data[2];
mynameisteodora 1:a192c8fd3da3 237 sum_x += xvals[i];
mynameisteodora 1:a192c8fd3da3 238 sum_y += yvals[i];
mynameisteodora 1:a192c8fd3da3 239 sum_z += zvals[i];
mynameisteodora 1:a192c8fd3da3 240 }
mynameisteodora 1:a192c8fd3da3 241
mynameisteodora 1:a192c8fd3da3 242 x_avg = sum_x/100.0f;
mynameisteodora 1:a192c8fd3da3 243 y_avg = sum_y/100.0f;
mynameisteodora 1:a192c8fd3da3 244 z_avg = sum_z/100.0f;
mynameisteodora 1:a192c8fd3da3 245
mynameisteodora 1:a192c8fd3da3 246 pc.printf("Accel_x average: %f\n", x_avg);
mynameisteodora 1:a192c8fd3da3 247 pc.printf("Accel_y average: %f\n", y_avg);
mynameisteodora 1:a192c8fd3da3 248 pc.printf("Accel_z average: %f\n", z_avg);
mynameisteodora 1:a192c8fd3da3 249
mynameisteodora 1:a192c8fd3da3 250 pc.printf("Accel bias: %f\n", accel_bias[0]);
mynameisteodora 1:a192c8fd3da3 251 pc.printf("Gyro bias: %f\n", gyro_bias[0]);
mynameisteodora 0:e1767e53fb0b 252
mynameisteodora 0:e1767e53fb0b 253 mpu9250.initMPU9250();
mynameisteodora 0:e1767e53fb0b 254 pc.printf("Initialisation successful!\n");
mynameisteodora 0:e1767e53fb0b 255
mynameisteodora 0:e1767e53fb0b 256 mpu9250.getAres();
mynameisteodora 0:e1767e53fb0b 257 pc.printf("Accel sensitivity: %f\n", aRes);
mynameisteodora 0:e1767e53fb0b 258
mynameisteodora 1:a192c8fd3da3 259 eventQueue.call_every(1000, periodicCallback);
mynameisteodora 0:e1767e53fb0b 260
mynameisteodora 0:e1767e53fb0b 261 BLE &ble = BLE::Instance();
mynameisteodora 0:e1767e53fb0b 262 pc.printf("BLE instance created!\n");
mynameisteodora 0:e1767e53fb0b 263 ble.onEventsToProcess(scheduleBleEventsProcessing);
mynameisteodora 0:e1767e53fb0b 264 pc.printf("BLE events scheduled!\n");
mynameisteodora 0:e1767e53fb0b 265 ble.init(bleInitComplete);
mynameisteodora 0:e1767e53fb0b 266 pc.printf("BLE init complete!\n");
mynameisteodora 0:e1767e53fb0b 267
mynameisteodora 0:e1767e53fb0b 268 eventQueue.dispatch_forever();
mynameisteodora 0:e1767e53fb0b 269 pc.printf("Dispatched");
mynameisteodora 0:e1767e53fb0b 270
mynameisteodora 0:e1767e53fb0b 271 return 0;
mynameisteodora 0:e1767e53fb0b 272
mynameisteodora 0:e1767e53fb0b 273
mynameisteodora 0:e1767e53fb0b 274 }
mynameisteodora 0:e1767e53fb0b 275
mynameisteodora 0:e1767e53fb0b 276
mynameisteodora 0:e1767e53fb0b 277
mynameisteodora 0:e1767e53fb0b 278
mynameisteodora 0:e1767e53fb0b 279