File content as of revision 22:50f88b18cfad:

#include <events/mbed_events.h>
#include <math.h> 

#include <ctime>
#include <mbed.h>
#include "MPU9250.h"
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/HeartRateService.h"

DigitalOut Led0(p31);
DigitalOut Led1(p30);
DigitalOut Led2(p29);
DigitalOut Led3(p28);
DigitalOut Led4(p4);
DigitalOut Led5(p3);
DigitalOut Led6(p11);
DigitalOut Led7(p12);
DigitalOut Led8(p13);
DigitalOut Led9(p14);
DigitalOut Led10(p15);
DigitalOut Led11(p16);
DigitalOut Led12(p17);
DigitalOut Led13(p18);
DigitalOut Led14(p19);
DigitalOut Led15(p20);
DigitalOut Led16(p22);
DigitalOut Led17(p23);
DigitalOut Led18(p24);
DigitalOut Led19(p25);

DigitalIn pb(p8);

int USV_count = 1;
int old_pb = 1;
int new_pb;
    
const static char     DEVICE_NAME[] = "ProVaida";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
short hrmCounter = 0;
static HeartRateService* hrService;

bool BLE_conn = false;

int16_t destination[3];
double step_threshold = 14.45;

double oldAcceleration = 0.0;
int callback_cycles = 1;
int step;
int step_buffer;
int totalsteps = 0;

int run_threshold = 5;
int run_count = 0;

MPU9250 mpu = MPU9250(P0_26, P0_27);

static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    BLE::Instance().gap().startAdvertising();
}

void updateSensorValue() {    
    wait(.001);
    new_pb = pb;
    if ((new_pb == 0) && (old_pb == 1)) {
        USV_count++;
        printf("count: %i\n", USV_count);
    }
    if (USV_count % 2) {        
        step = 0;
        callback_cycles++;
    
        mpu.readAccelData(destination);
    
        double acc_x = destination[0] / 1000.0;
        double acc_y = destination[1] / 1000.0;
        double acc_z = destination[2] / 1000.0;
    
        double sqr_acc_x = acc_x*acc_x;
        double sqr_acc_y = acc_y*acc_y;
        double sqr_acc_z = acc_z*acc_z;
    
        double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
        double accel = sqrt(sum_acc);
    
        if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
            step_buffer++;
            //reached running speed
            if (callback_cycles <= run_threshold) {
                if (run_count >= 2) {
                    step = 2;
                }
                else {
                    step = 1;
                    run_count++;
                }
            }
            //at walking speed
            else {
                step = 1;
                run_count = 0;    
            }
            callback_cycles = 0;
        }
        if (step != 0)
        {
            totalsteps++;                
            Led0 = ((totalsteps >> 0) % 2);
            Led1 = ((totalsteps >> 1) % 2);
            Led2 = ((totalsteps >> 2) % 2);
            Led3 = ((totalsteps >> 3) % 2);
            Led4 = ((totalsteps >> 4) % 2);
            Led5 = ((totalsteps >> 5) % 2);
            Led6 = ((totalsteps >> 6) % 2);
            Led7 = ((totalsteps >> 7) % 2);
            Led8 = ((totalsteps >> 8) % 2);
            Led9 = ((totalsteps >> 9) % 2);
            Led10 = ((totalsteps >> 10) % 2);
            Led11 = ((totalsteps >> 11) % 2);
            Led12 = ((totalsteps >> 12) % 2);
            Led13 = ((totalsteps >> 12) % 2);
            Led14 = ((totalsteps >> 14) % 2);
            Led15 = ((totalsteps >> 15) % 2);
            Led16 = ((totalsteps >> 16) % 2);
            Led17 = ((totalsteps >> 17) % 2);
            Led18 = ((totalsteps >> 18) % 2);
            Led19 = (step == 2);
            
            if ((totalsteps >> 19) % 2) {
                totalsteps = 0;
            }   
        }
        oldAcceleration = accel;

    } else if (totalsteps != 0) {
       totalsteps = 0;
    }         
    old_pb = new_pb;    
    
    if (BLE_conn) {
        step_buffer--;
        if (step_buffer > 0) {
            hrService->updateHeartRate(-1);
            while (step_buffer > 127) {
                hrService->updateHeartRate(127);
                step_buffer -= 127;
            }
            hrService->updateHeartRate(step_buffer);
            step_buffer = 0;
            hrService->updateHeartRate(-1);
        }
        hrmCounter = (short) step;    
        hrService->updateHeartRate(hrmCounter);        
        BLE_conn = false;
    }
}

void blinkCallback(void)
{
    BLE &ble = BLE::Instance();
    if (ble.gap().getState().connected) {
        BLE_conn = true;        
    }
    eventQueue.call(updateSensorValue);

}

void onBleInitError(BLE &ble, ble_error_t error) {}

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

    hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);

    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::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); 
    ble.gap().startAdvertising();
}

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

int main()
{   
    
    pb.mode(PullUp);
    mpu.initMPU9250();
    
    eventQueue.call_every(80, blinkCallback);

    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);

    eventQueue.dispatch_forever();

    return 0;
}