File content as of revision 3:34f974c907d3:

#include "mbed.h"
#include "ble/BLE.h"
#include "functions.h"
#include "lsm6ds0.h"
#include "ButtonService.h"

static Serial pc(SERIAL_TX, SERIAL_RX);


InterruptIn LSM6DS0_int(D4);
DigitalOut led(LED1);
LSM6DS0 *gyro_accel;
xl_output data_FIFO[FIFO_length_by_five];
int offset;

BLE         ble;
const static char     DEVICE_NAME[] = "StepCounter";
static const uint16_t uuid16_list[] = {StepCounterService::STEPCOUNTER_SERVICE_UUID};


StepCounterService *StepCounterServicePtr;









static volatile bool  triggerSensorPolling = false;
static uint8_t StepCounter = 0;
//   Restart advertising when phone app disconnects
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    ble.gap().startAdvertising();
    
}

void get_data() {
    LSM6DS0_int.disable_irq(); //Disable IRQ interrupt
    led = !led; //Blink led
    gyro_accel->storeFIFO(&offset, data_FIFO); //Store the data from FIFO to data_FIFO variable
    LSM6DS0_int.enable_irq(); //Re-enable IRQ interrupt
}


void periodicCallback(void)
{
    
    /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
     * heavy-weight sensor polling from the main thread. */
    triggerSensorPolling = true;
}

//  Initialization callback
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE &ble          = params->ble;
    ble_error_t error = params->error;
    
    if (error != BLE_ERROR_NONE) {
        return;
    }
    ble.gap().onDisconnection(disconnectionCallback);
    
    StepCounterService StepCounterService(ble, false);    /* initial value for button pressed */
    StepCounterServicePtr = &StepCounterService;
    
    /* Setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); // BLE only, no classic BT
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); // advertising type
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); // add name
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); // UUID's broadcast in advertising packet
    ble.gap().setAdvertisingInterval(100); // 100ms.
    
    /* Start advertising */
    ble.gap().startAdvertising();
}

int main()
{
    LSM6DS0_ACC_ODR_t odr;
    LSM6DS0_ACC_HR_t high_res;
    LSM6DS0_FIFO_mode_t work_mode;
    
    offset = 0; //Offset in the data_FIFO vector. It is useful to store the FIFO value in the data_FIFO vector without overwritting (after 5 FIFO value the data_FIFO vector has been overwritten)
    odr = LSM6DS0_ACC_ODR_50Hz ;
    high_res = LSM6DS0_ACC_HR_Disabled;
    work_mode = LSM6DS0_FIFO_mode_CONTINUOUS;
    
    LSM6DS0_int.rise(&get_data); //LSM6DS0's interrupt initializzation
    gyro_accel->LSM6DS0_reset(); //LSM6DS0 reset
    gyro_accel->Init(odr, high_res, work_mode); //LSM6DS0 initializzation
    
    Ticker ticker;
    ticker.attach(periodicCallback, 1);
    
    ble.init(bleInitComplete);
    
    /* SpinWait for initialization to complete. This is necessary because the
     * BLE object is used in the main loop below. */
    while (ble.hasInitialized()  == false) { /* spin loop */ }
    
    /* Infinite loop waiting for BLE interrupt events */
    while (1)
    {
        // check for trigger from periodicCallback()
        if (triggerSensorPolling && ble.getGapState().connected)
        {
            triggerSensorPolling = false;
            
            // Do blocking calls or whatever is necessary for sensor polling.
            // In our case, we simply update the HRM measurement.
            StepCounter++;
            StepCounterServicePtr->updateStepCounterState(StepCounter);
        } else {
            ble.waitForEvent(); // low power wait for event
        }
    }
    
}