File content as of revision 0:e1767e53fb0b:

#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"


DigitalOut led1(LED1, 1);

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

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;

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

uint8_t step  = 0; 
uint8_t test = 0;

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

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};
    mpu9250.readAccelData(accel_data);
    pc.printf("Accel_x: %d\n", accel_data[0]);
    mpu9250.readGyroData(gyro_data);
    pc.printf("Gyro_x: %d\n", gyro_data[0]);
    //pc.printf("Reading values\n");
//        mpu9250.readAccelData(accel_data);
//        ax = (float)accel_data[0]*aRes;  // get actual g value, this depends on scale being set
//        ay = (float)accel_data[1]*aRes;   
//        az = (float)accel_data[2]*aRes;  
//        pc.printf("accel_x = %f\n",ax);
//        pc.printf("accel_y = %f\n",ay);
//        pc.printf("aceel_z = %f\n",az);
//        mag_accel = sqrt(ax*ax + ay*ay + az*az);
//        pc.printf("Magnitude of acceleration: %f\n", mag_accel);
//        
//        mpu9250.readGyroData(gyro_data);
//        gx = (float)gyro_data[0]*gRes - gyro_bias[0];
//        gy = (float)gyro_data[1]*gRes - gyro_bias[1];
//        gz = (float)gyro_data[2]*gRes - gyro_bias[2];
////      pc.printf("gyro_x = %f\n",gx);
////      pc.printf("gyro_y = %f\n",gy);
////      pc.printf("gyro_z = %f\n",gz);
//        mag_gyro = sqrt(gx*gx + gy*gy + gz*gz);
//      pc.printf("Magnitude of gyro: %f\n", mag_gyro);
//    //  mag_accel_int = (uint8_t)mag_accel * 1000;
//        
        
        // If the acceleration vector is greater than 0.15, add the steps
        if(mag_accel  > 0.15f) {
            if(test > 254) {
                test = 0;
            }
            test++;
            step  = step  + 2; 
        }
                              
        // Avoiding counting the same steps twice                                       
        //wait(0.65);
                

    hrServicePtr->updateHeartRate(test);
    pc.printf("Sensor value updated!\n");
}

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);
    pc.printf("Hello World!\n");
    
    mpu9250.resetMPU9250();
    pc.printf("MPU reset\n");
    
    mpu9250.calibrateMPU9250(accel_bias, gyro_bias);
    pc.printf("Calibration done!\n");
    
    pc.printf("%f\n", accel_bias[0]);
    pc.printf("%f\n", gyro_bias[0]);
    
    mpu9250.initMPU9250();
    pc.printf("Initialisation successful!\n");
    
    mpu9250.getAres();
    pc.printf("Accel sensitivity: %f\n", aRes);
    
    eventQueue.call_every(100, 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;

    
}