Revision 2:6d3c59b866aa, committed 2019-01-17

Comitter:

mynameisteodora

Date:

Thu Jan 17 23:27:17 2019 +0000

Parent:

1:a192c8fd3da3

Commit message:

Final working version

Changed in this revision

--- a/main.cpp	Tue Nov 13 23:52:30 2018 +0000
+++ b/main.cpp	Thu Jan 17 23:27:17 2019 +0000
@@ -6,6 +6,7 @@
 #include "ble/BLE.h"
 #include "ble/Gap.h"
 #include "ble/services/HeartRateService.h"
+#include "us_ticker_api.h"
 
 
 DigitalOut led1(LED1, 1);
@@ -13,7 +14,6 @@
 const static char     DEVICE_NAME[] = "Pedometer";
 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
 
-//static uint8_t hrmCounter = 100; // init HRM to 100bps
 static HeartRateService *hrServicePtr;
 
 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
@@ -35,118 +35,225 @@
 uint8_t test = 0;
 
 float x_avg, y_avg, z_avg;
+
+// threshold should be dynamic, obtained by (max+min)/2
 float threshold = 80.0f;
 
+float dynamic_threshold_x = 80.0f;
+float dynamic_threshold_y = 80.0f;
+float dynamic_threshold_z = 80.0f;
+
+// the first time the low_pass function is called, xm1 is 0 
+// then it will be updated with values from the previous call
+float xm1_x = 0, xm1_y = 0, xm1_z = 0;
+float max_lp = 120.0f;
+float min_lp = -40.0f;
+float threshold_lp = 80.0f;
+
+
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     BLE::Instance().gap().startAdvertising(); // restart advertising
 }
 
+// simple low-pass filter with two registers
+float low_pass(float *x, float *y, int M, float xm1) {
+    int n;
+    y[0] = x[0] + xm1;
+    for(n = 1; n < M; n++) {
+        y[n] = x[n] + x[n-1];
+    }
+    return x[M-1];
+}
+
+
 void updateSensorValue() {
-    // Do blocking calls or whatever is necessary for sensor polling.
-    // In our case, we simply update the HRM measurement.
+
     int16_t accel_data[3] = {0};
-    int16_t gyro_data[3] = {0};
-    float ax, ay, az;
-    float magnitude[100] = {0};
-    float rolling_average[100] = {0};
-    short flag = 0;
-    
-    pc.printf("Accel_x average: %f\n", x_avg);
-    pc.printf("Accel_y average: %f\n", y_avg);
-    pc.printf("Accel_z average: %f\n", z_avg);
-    
-    mpu9250.readAccelData(accel_data);
-    pc.printf("Accel_x: %d\n", accel_data[0]);
-    pc.printf("Accel_y: %d\n", accel_data[1]);
-    pc.printf("Accel_z: %d\n", accel_data[2]);
-    
-    mpu9250.readGyroData(gyro_data);
-    pc.printf("Gyro_x: %d\n", gyro_data[0]);
-    pc.printf("Gyro_y: %d\n", gyro_data[1]);
-    pc.printf("Gyro_z: %d\n", gyro_data[2]);
-    
-    ax = accel_data[0] - x_avg;
-    ay = accel_data[1] - y_avg;
-    az = accel_data[2] - z_avg;
-    
-    magnitude[0] = sqrt(ax*ax + ay*ay + az*az);
-    
-    for(int i = 1; i < 100; i++) {
-        step = 0;
+
+    int window_size = 20;
+    int M = window_size/2;
+    // 0 - x, 1 - y, 2 - z
+    int most_active_axis = 0;
+    int flag = 0;
+
+    float in_x[window_size];
+    float in_y[window_size];
+    float in_z[window_size];
+
+    float out_x[window_size];
+    float out_y[window_size];
+    float out_z[window_size];
+
+
+    // collect n = window_size samples and detect most active axis
+    float min_x = 0.0f, min_y = 0.0f, min_z = 0.0f;
+    float max_x = 0.0f, max_y = 0.0f, max_z = 0.0f;
+    float peak_to_peak_x = 0.0f, peak_to_peak_y = 0.0f, peak_to_peak_z = 0.0f;
+
+    for(int i = 0; i < window_size; i++) {
         mpu9250.readAccelData(accel_data);
-        pc.printf("Accel_x: %d\n", accel_data[0]);
-        pc.printf("Accel_y: %d\n", accel_data[1]);
-        pc.printf("Accel_z: %d\n", accel_data[2]);
-    
-        mpu9250.readGyroData(gyro_data);
-        pc.printf("Gyro_x: %d\n", gyro_data[0]);
-        pc.printf("Gyro_y: %d\n", gyro_data[1]);
-        pc.printf("Gyro_z: %d\n", gyro_data[2]);
+        
+        in_x[i] = accel_data[0] - x_avg;
+        in_y[i] = accel_data[1] - y_avg;
+        in_z[i] = accel_data[2] - z_avg;
         
-        ax = accel_data[0] - x_avg;
-        ay = accel_data[1] - y_avg;
-        az = accel_data[2] - z_avg;
-    
-        magnitude[i] = sqrt(ax*ax + ay*ay + az*az);
-        rolling_average[i] = (magnitude[i] - magnitude[i-1])/2;
-        pc.printf("Rolling average: %f\n", rolling_average[i]);
-        
-        if (rolling_average[i]>threshold && flag==0) {
-            // a step has been taken
-            step = 1;
-            hrServicePtr->updateHeartRate(step);
-            pc.printf("Sensor value updated with value: %d\n", step);
-            flag=1;
+        pc.printf("Accel_x: %d\n", in_x[i]);
+        pc.printf("Accel_y: %d\n", in_y[i]);
+        pc.printf("Accel_z: %d\n", in_z[i]);
+
+        if(in_x[i] > max_x) {
+            max_x = in_x[i];
+        }
+
+        if(in_y[i] > max_y) {
+            max_y = in_y[i];
+        } 
+
+        if(in_z[i] > max_z) {
+            max_z = in_z[i];
+        } 
+
+        if(in_x[i] < min_x) {
+            min_x = in_x[i];
         }
 
-        else if (rolling_average[i] > threshold && flag==1) {
-      
-            //do nothing 
+        if(in_y[i] < min_y) {
+            min_y = in_y[i];
+        } 
+
+        if(in_z[i] < min_z) {
+            min_z = in_z[i];
         }
+    }
 
-        if (rolling_average[i] <threshold  && flag==1) {
-            flag=0;
+    peak_to_peak_x = max_x - min_x;
+    peak_to_peak_y = max_y - min_y;
+    peak_to_peak_z = max_z - max_z;
+
+    if(peak_to_peak_x > peak_to_peak_y) {
+        if(peak_to_peak_x > peak_to_peak_z) {
+            dynamic_threshold_x = (min_x + max_x)/2;
+            most_active_axis = 0;
+            pc.printf("Most active axis: X\n");
+        }
+        else {
+            dynamic_threshold_z = (min_z + max_z)/2;
+            most_active_axis = 2;
+            pc.printf("Most active axis: Z\n");
         }
     }
- //   mpu9250.readAccelData(accel_data);
-//    pc.printf("Accel_x: %d\n", accel_data[0]);
-//    pc.printf("Accel_y: %d\n", accel_data[1]);
-//    pc.printf("Accel_z: %d\n", accel_data[2]);
-//    
-//    mpu9250.readGyroData(gyro_data);
-//    pc.printf("Gyro_x: %d\n", gyro_data[0]);
-//    pc.printf("Gyro_y: %d\n", gyro_data[1]);
-//    pc.printf("Gyro_z: %d\n", gyro_data[2]);
-//    
-//    og_mag_accel = accel_data[0]*accel_data[0] + accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2];
-//    pc.printf("Magnitude of accel: %d\n", og_mag_accel);
-//    
-//    pc.printf("Accel Res: %f\n", aRes);
-//    
-//    threshold = (0.15f * 0.15f)*16384*16384;
-//    pc.printf("Threshold: %f\n", threshold);
-//
-//    // If the acceleration vector is greater than 0.15, add the steps
-//    if(og_mag_accel  > (int) threshold) {
-//        if(test > 254) {
-//            test = 0;
-//        }
-//            test++;
-//            step  = step  + 2; 
-//        }
-//                              
-//        // Avoiding counting the same steps twice                                       
-//        //wait(0.65);
-//                
+    else if(peak_to_peak_y > peak_to_peak_z) {
+        dynamic_threshold_y = (min_y + max_y)/2;
+        most_active_axis = 1;
+        pc.printf("Most active axis: Y\n");
+    }
+    else {
+        dynamic_threshold_z = (min_z + max_z)/2;
+        most_active_axis = 2;
+        pc.printf("Most active axis: Z\n");
+    }
+
+    if(most_active_axis == 0) {
+        // low pass on x-axis
+        xm1_x = low_pass(in_x, out_x, M, xm1_x);
+        xm1_x = low_pass(&in_x[M], &out_x[M], M, xm1_x);
+        pc.printf("Low passed axis X\n");
+
+        // now analyse the output data, out_x, to see if the threshold has been passed
+        for(int i = 1; i < window_size; i++) {
+            
+            // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
+            if(out_x[i] < out_x[i-1] && out_x[i] < dynamic_threshold_x && out_x[i-1] > dynamic_threshold_x && flag == 0) {
+                
+                step = 1;
+                flag = 1;
+                pc.printf("Step!\n");
+                hrServicePtr->updateHeartRate(step);
+                wait(0.50);
+
+            }
+        
+            else if (out_x[i] < out_x[i-1] && out_x[i] < dynamic_threshold_x && out_x[i-1] > dynamic_threshold_x && flag == 1) {
+            // do nothing
+            }
+            
+            // if the threshold is being crossed from the lower half to the upper half and the flag is set to 1
+            else if (out_x[i] > out_x[i-1] && out_x[i] > dynamic_threshold_x && out_x[i-1] < dynamic_threshold_x && flag == 1) {
+            // this is a step but we are counting gaits
+            // however, we need to set the flag to 0 
+                flag = 0;
+            }
+       } 
+    }
+
+    else if(most_active_axis == 1) {
+        // low pass on y-axis
+        xm1_y = low_pass(in_y, out_y, M, xm1_y);
+        xm1_y = low_pass(&in_y[M], &out_y[M], M, xm1_y);
+        pc.printf("Low passed axis Y\n");
 
+        // now analyse the output data, out_y, to see if the threshold has been passed
+        for(int i = 1; i < window_size; i++) {
+            
+            // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
+            if(out_y[i] < out_y[i-1] && out_y[i] < dynamic_threshold_y && out_y[i-1] > dynamic_threshold_y && flag == 0) {
+                
+                step = 1;
+                flag = 1;
+                pc.printf("Step!\n");
+                hrServicePtr->updateHeartRate(step);
+                wait(0.50);
+
+            }
+            
+            else if (out_y[i] < out_y[i-1] && out_y[i] < dynamic_threshold_y && out_y[i-1] > dynamic_threshold_y && flag == 1) {
+            // do nothing
+            }
+            
+            // if the threshold is being crossed from the lower half to the upper half and the flag is set to 1
+            else if (out_y[i] > out_y[i-1] && out_y[i] > dynamic_threshold_y && out_y[i-1] < dynamic_threshold_y && flag == 1) {
+            // this is a step but we are counting gaits
+            // however, we need to set the flag to 0 
+                flag = 0;
+            }
+       } 
+    }
+    else if(most_active_axis == 2) {
+        // low pass on z-axis
+        xm1_z = low_pass(in_z, out_z, M, xm1_z);
+        xm1_z = low_pass(&in_z[M], &out_z[M], M, xm1_z);
+        
+        pc.printf("Low passed axis Z\n");
+
+        // now analyse the output data, out_z, to see if the threshold has been passed
+        for(int i = 1; i < window_size; i++) {
+            
+            // if the threshold is being crossed from the upper half to the lower half and the flag is set to 0
+            if(out_z[i] < out_z[i-1] && out_z[i] < dynamic_threshold_z && out_z[i-1] > dynamic_threshold_z && flag == 0) {
+                
+                step = 1;
+                flag = 1;
+                pc.printf("Step!\n");
+                hrServicePtr->updateHeartRate(step);
+                wait(0.50);
+            }
+        
+
+        
+            else if (out_z[i] < out_z[i-1] && out_z[i] < dynamic_threshold_z && out_z[i-1] > dynamic_threshold_z && flag == 1) {
+            // do nothing
+            }
+        
+            else if (out_z[i] > out_z[i-1] && out_z[i] > dynamic_threshold_z && out_z[i-1] < dynamic_threshold_z && flag == 1) {
+            // this is a step but we are counting gaits
+            // however, we need to set the flag to 0 
+                flag = 0;
+            }
+       } 
+    }
+    }
     
-//    
-//    step++;
-//    
-//    hrServicePtr->updateHeartRate(step);
-//    pc.printf("Sensor value updated AGAIN with value: %d\n", step);
-}
 
 void periodicCallback(void)
 {
@@ -215,7 +322,6 @@
 
 int main() {
     pc.baud(9600);
-    pc.printf("Hello World!\n");
     
     mpu9250.resetMPU9250();
     pc.printf("MPU reset\n");
@@ -223,7 +329,7 @@
     mpu9250.calibrateMPU9250(accel_bias, gyro_bias);
     pc.printf("Library calibration done!\n");
     
-    // try "calibrating" to get the threshold
+    // Implement own calibration to estimate the threshold values
     int sum_x = 0, sum_y = 0, sum_z = 0;
     int xvals[100] = {0}, yvals[100] = {0}, zvals[100] = {0};
     int16_t local_accel_data[3] = {0};
@@ -248,7 +354,6 @@
     pc.printf("Accel_z average: %f\n", z_avg);
     
     pc.printf("Accel bias: %f\n", accel_bias[0]);
-    pc.printf("Gyro bias: %f\n", gyro_bias[0]);
     
     mpu9250.initMPU9250();
     pc.printf("Initialisation successful!\n");
@@ -256,6 +361,7 @@
     mpu9250.getAres();
     pc.printf("Accel sensitivity: %f\n", aRes);
     
+    // the sensor readings are updated every second
     eventQueue.call_every(1000, periodicCallback);
 
     BLE &ble = BLE::Instance();
@@ -275,5 +381,3 @@
 
 
 
-
-