File content as of revision 2:6d3c59b866aa:

#include "mbed.h"
#include "inttypes.h"
#include "MPU9250.h"
#include "math.h"
#include <events/mbed_events.h>
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/HeartRateService.h"
#include "us_ticker_api.h"


DigitalOut led1(LED1, 1);

const static char     DEVICE_NAME[] = "Pedometer";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};

static HeartRateService *hrServicePtr;

static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);

MPU9250 mpu9250(P0_26, P0_27);


Serial pc(USBTX, USBRX); // tx, rx

float accel_bias[3], gyro_bias[3];
float ax, ay, az, gx, gy, gz, mag_accel, mag_gyro;
short mag_accel_int = 0;
int og_mag_accel;

float aRes = mpu9250.aRes;
float gRes = mpu9250.gRes;

uint8_t step = 0; 
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() {

    int16_t accel_data[3] = {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);
        
        in_x[i] = accel_data[0] - x_avg;
        in_y[i] = accel_data[1] - y_avg;
        in_z[i] = accel_data[2] - z_avg;
        
        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];
        }

        if(in_y[i] < min_y) {
            min_y = in_y[i];
        } 

        if(in_z[i] < min_z) {
            min_z = in_z[i];
        }
    }

    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");
        }
    }
    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;
            }
       } 
    }
    }
    

void periodicCallback(void)
{
    led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */

    if (BLE::Instance().getGapState().connected) {
        eventQueue.call(updateSensorValue);
    }
}

void onBleInitError(BLE &ble, ble_error_t error)
{
    (void)ble;
    (void)error;
   /* Initialization error handling should go here */
}

void printMacAddress()
{
    /* Print out device MAC address to the console*/
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    printf("DEVICE MAC ADDRESS: ");
    for (int i = 5; i >= 1; i--){
        printf("%02x:", address[i]);
    }
    printf("%02x\r\n", address[0]);
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        onBleInitError(ble, error);
        return;
    }

    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service. */
    hrServicePtr = new HeartRateService(ble, mag_accel_int, HeartRateService::LOCATION_FINGER);

    /* Setup advertising. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
    ble.gap().startAdvertising();

    printMacAddress();
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

int main() {
    pc.baud(9600);
    
    mpu9250.resetMPU9250();
    pc.printf("MPU reset\n");
    
    mpu9250.calibrateMPU9250(accel_bias, gyro_bias);
    pc.printf("Library calibration done!\n");
    
    // 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};
    
    
    for(int i = 0; i < 100; i++) {
        mpu9250.readAccelData(local_accel_data);
        xvals[i] = local_accel_data[0];
        yvals[i] = local_accel_data[1];
        zvals[i] = local_accel_data[2];
        sum_x += xvals[i];
        sum_y += yvals[i];
        sum_z += zvals[i];
    }
    
    x_avg = sum_x/100.0f;
    y_avg = sum_y/100.0f;
    z_avg = sum_z/100.0f;
    
    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);
    
    pc.printf("Accel bias: %f\n", accel_bias[0]);
    
    mpu9250.initMPU9250();
    pc.printf("Initialisation successful!\n");
    
    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();
    pc.printf("BLE instance created!\n");
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    pc.printf("BLE events scheduled!\n");
    ble.init(bleInitComplete);
    pc.printf("BLE init complete!\n");

    eventQueue.dispatch_forever();
    pc.printf("Dispatched");
    
    return 0;

    
}