File content as of revision 10:d00d09bc31d6:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  Fabio Brembilla
 * @version V2.0.0
 * @date    May, 2016
 * @brief   SunTracker + BLE (Client) Vertical Application
 *          This application use IKS01A1, IDB0XA1 expansion boards
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
 */

/* Define --------------------------------------------------------------------*/

#define Sensors     //IKS01A1 Option

/* Includes ------------------------------------------------------------------*/

#include "mbed.h"

#ifdef Sensors
    #include "DevI2C.h"
    #include "x_nucleo_iks01a1.h"
#endif

/* BlueTooth -----------------------------------------------------------------*/

#include "BLE.h"
#include "UUID.h"
#include "DiscoveredCharacteristic.h"
#include "DiscoveredService.h"
#include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32

#define SCAN_INT  0x30 // 30 ms = 48 * 0.625 ms
#define SCAN_WIND 0x30 // 30 ms = 48 * 0.625 ms

// Not need to re-define it, it is already defined into UUID.h, just use UUID:: when you call it
//const unsigned   LENGTH_OF_LONG_UUID = 16;
//typedef uint16_t ShortUUIDBytes_t;
//typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];

const Gap::Address_t  BLE_address_BE       = {0xCC, 0x06, 0x05, 0x04, 0xCB, 0xCA};  // CLIENT address
const Gap::Address_t  BLE_peer_address_BE  = {0xFC, 0x03, 0x02, 0x01, 0xFB, 0xFA};  // SERVER address (must be the same of SERVER)

//const UUID::ShortUUIDBytes_t CONTROL_COMMAND_CHAR_UUID  = 0xA001;
//const UUID::ShortUUIDBytes_t SENS_STATUS_CHAR_UUID      = 0xB001;
//const UUID::ShortUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID  = 0xB002;
//const UUID::ShortUUIDBytes_t SENS_POSITION_CHAR_UUID    = 0xB003;
//const UUID::ShortUUIDBytes_t SENS_SUNPANEL_CHAR_UUID    = 0xB004;
const UUID::LongUUIDBytes_t CONTROL_COMMAND_CHAR_UUID = {0x00,0x00,0x00,0x02,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
const UUID::LongUUIDBytes_t SENS_STATUS_CHAR_UUID     = {0x00,0x00,0x08,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
const UUID::LongUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID = {0x00,0x00,0x10,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
const UUID::LongUUIDBytes_t SENS_POSITION_CHAR_UUID   = {0x00,0x00,0x20,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
const UUID::LongUUIDBytes_t SENS_SUNPANEL_CHAR_UUID   = {0x00,0x00,0x40,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};

DiscoveredCharacteristic command_bleCharacteristic;
DiscoveredCharacteristic status_bleCharacteristic;
DiscoveredCharacteristic difference_bleCharacteristic;
DiscoveredCharacteristic position_bleCharacteristic;
DiscoveredCharacteristic sunpanel_bleCharacteristic;

#define COMMAND_DATA_LEN        2+4+1+2 // TimeStamp (only for Response) + Feature + Type + Data
// 1 byte   0xFF     8 bit      int8
// 2 byte   0xFFFF   16 bit     int16

uint8_t value_write[COMMAND_DATA_LEN];

#define FeatureStatus      0x00000800
#define FeatureDifference  0x00001000
#define FeaturePosition    0x00002000
#define FeatureSunPanel    0x00004000

/* Variables ------------------------------------------------------------------*/

enum {
  IDLE = 0,
  READ_STA  = 1,
  READ_DIF  = 2,
  READ_POS  = 3,
  READ_SUN  = 4,
  WRITE_STA = 5,
  WRITE_DIF = 6
};
int trigger_Op = IDLE;
bool trigger_button = false;
int16_t Status_to_Write=0;  // to avoid to read the old Status by BLE before send the new Status by BLE

int16_t Status=0;      // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Receive BLE]
int16_t diff=0;        // Babybear or Accelerometer difference [--> Receive BLE]
int16_t pos=0;         // Motor Position [--> Receive BLE]
int16_t measure=0;     // ADC Value from SunPanel [--> Receive BLE]

/* Initializations ------------------------------------------------------------*/

#ifdef Sensors

// Initializing I2C bus
DevI2C dev_i2c(D14, D15);

// Initializing Sensors Component IKS01A1
static X_NUCLEO_IKS01A1 *mems;
MotionSensor *accelerometer;

int32_t acc_data[3];    // Accelerometer difference
int16_t acc_diff=0;     // Accelerometer difference

#endif

DigitalOut myled(LED1, 0);
InterruptIn mybutton(USER_BUTTON);

/* User_Button_Pressed -------------------------------------------------------*/

void User_Button_Pressed(void)
{ 

    trigger_button = true;
    Status++;
    Status_to_Write = Status;

#ifdef Sensors
    if (Status_to_Write>3)  { Status=1; Status_to_Write=1; }
#else
    if (Status_to_Write>2)  { Status=1; Status_to_Write=1; }
#endif 
    
}

/* Bluetooth CallBack ---------------------------------------------------------*/

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{    
    if (params->peerAddr[0] != BLE_peer_address_BE[0]) // return if miss the server MAC address
    { 
        printf("Missing Expected MAC Address\n\r");
        return; // exit from advertisementCallback
    }
    
//    printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u\r\n",
//           params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
//           params->rssi, params->isScanResponse, params->type);

    printf("Found Expected MAC Address: isScanResponse %u, AdvertisementType %u\r\n",params->isScanResponse, params->type);
    
    if(!params->isScanResponse)
    {
      BLE::Instance().gap().connect(params->peerAddr, Gap::ADDR_TYPE_PUBLIC, NULL, NULL);
    }
}

void serviceDiscoveryCallback(const DiscoveredService *service)
{
    printf("Start Service Discovery\r\n");

    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT)
    {
        printf("S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
    }
    else
    {
        printf("S UUID-");
        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++)
        {
            printf("%02X", longUUIDBytes[i]);
        }
        printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
    }
  
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
{   
    int COMMAND_CHAR_count=0, STATUS_CHAR_count=0, DIFFERENCE_CHAR_count=0, POSITION_CHAR_count=0, SUNPANEL_CHAR_count=0;
    const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
    
    for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++)
    { 
        if (longUUIDBytes[i] == CONTROL_COMMAND_CHAR_UUID[i])   COMMAND_CHAR_count++;
        if (longUUIDBytes[i] == SENS_STATUS_CHAR_UUID[i])       STATUS_CHAR_count++;
        if (longUUIDBytes[i] == SENS_DIFFERENCE_CHAR_UUID[i])   DIFFERENCE_CHAR_count++;
        if (longUUIDBytes[i] == SENS_POSITION_CHAR_UUID[i])     POSITION_CHAR_count++;
        if (longUUIDBytes[i] == SENS_SUNPANEL_CHAR_UUID[i])     SUNPANEL_CHAR_count++;
    }
    
    //if (characteristicP->getUUID().getShortUUID() == CONTROL_COMMAND_CHAR_UUID)
    if (COMMAND_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
    {
        command_bleCharacteristic = *characteristicP;
        printf("Found Command Characteristic\r\n");
    }

    //if (characteristicP->getUUID().getShortUUID() == SENS_STATUS_CHAR_UUID)
    if (STATUS_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
    {
        status_bleCharacteristic = *characteristicP;    
        printf("Found Status Characteristic\r\n");
    }

    //if (characteristicP->getUUID().getShortUUID() == SENS_DIFFERENCE_CHAR_UUID)    
    if (DIFFERENCE_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
    {
        difference_bleCharacteristic = *characteristicP;    
        printf("Found Difference Characteristic\r\n");
    }

    //if (characteristicP->getUUID().getShortUUID() == SENS_POSITION_CHAR_UUID)    
    if (POSITION_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
    {
        position_bleCharacteristic = *characteristicP;
        printf("Found Position Characteristic\r\n");
    }

    //if (characteristicP->getUUID().getShortUUID() == SENS_SUNPANEL_CHAR_UUID)    
    if (SUNPANEL_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
    {
        sunpanel_bleCharacteristic = *characteristicP;
        printf("Found Sunpanel Characteristic\r\n");
    }
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
{
    //printf("discoveryTerminationCallback for handle %u\r\n", connectionHandle);
    printf("Stop Service Discovery\r\n\r\n");
    
    trigger_Op = READ_STA;
}

void onDataReadCallback(const GattReadCallbackParams *response)
{
    // Read SERVER --> CLIENT
    
    // From SERVER it receives a sensor in this format: TimeStamp (2) + Data (2)
    //uint16_t TimeStamp = response->data[0]<<8 | response->data[1];
    int16_t Data = response->data[2] | response->data[3]<<8;       // Return original value after inverted with STORE_LE_16 into SERVER before send by BLE
    
    //printf("\r\n\ronDataReadCallback (TimeStamp %x)", TimeStamp);
    //printf("\n\ronDataReadCallback (Data Hex %x)", Data);
    //printf("\n\ronDataReadCallback (Data Dec %d)", Data);   
    
    if (response->handle == status_bleCharacteristic.getValueHandle())
    {     
        Status = Data;         
        printf("\n\rReceive BLE Display Status %d\n\r", Status);
        
        trigger_Op = READ_DIF;
    }
    
    if (response->handle == difference_bleCharacteristic.getValueHandle())
    {    
        diff = Data;      
        printf("Receive BLE Difference %d lux/mems\n\r", diff);
        
        trigger_Op = READ_POS;
    }
    
    if (response->handle == position_bleCharacteristic.getValueHandle())
    {   
        pos = Data;
        printf("Receive BLE Position %d\n\r", pos);
        
        trigger_Op = READ_SUN;
    }
    
    if (response->handle == sunpanel_bleCharacteristic.getValueHandle())
    {   
        measure = Data;         
        printf("Receive BLE Sunpanel %d mV\n\r", measure);
        
        trigger_Op = WRITE_STA;
    }
}

void myonDataWriteCallback(const GattWriteCallbackParams *response)
{
    // Write CLIENT --> SERVER

    if (response->handle == command_bleCharacteristic.getValueHandle())
    {

        // From CLIENT it writes a command in this format: Feature (4) + Type (1) + Data (2)        
        uint32_t Feature = response->data[0]<<24 | response->data[1]<<16 | response->data[2]<<8 | response->data[3];
        //uint8_t Type = response->data[4];
        int16_t Data = response->data[5]<<8 | response->data[6];
            
        //printf("\r\nmyonDataWriteCallback (Feature %x)", Feature);
        //printf("\r\nmyonDataWriteCallback (Type %x)", Type);        // Not Used
        //printf("\r\nmyonDataWriteCallback (Data Hex %x)", Data);
        //printf("\r\nmyonDataWriteCallback (Data Dec %d)", Data);

        switch(Feature)
        {
            case FeatureStatus:
                printf("\r\nSend STATUS %d\r\n", Data);
                Status = Data;
                trigger_Op = WRITE_DIF;
                break;
     
            case FeatureDifference:
                printf("\r\nSend DIFF %d\r\n", Data);
                diff = Data;
                trigger_Op = READ_STA;
                break;
 
            case FeaturePosition:
                break;
            
            case FeatureSunPanel:
                break;
                
            default:
                break;
        }
        
    }

}

/** 
 * This function is called when the ble initialization process has failled 
 */ 
void onBleInitError(BLE &ble, ble_error_t error) 
{ 
    /* Initialization error handling should go here */ 
    
    printf("Inizialization Error\n\r");
}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    // Good only for Short UUID
    //uint16_t CONTROL_SERVICE_UUID = 0x00,0x00,0x00,0x00,0x00,0x0F,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t COMMAND_CHARACTERISTIC_UUID = 0x00,0x00,0x00,0x02,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t SENSORS_SERVICE_UUID = 0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t STATUS_CHARACTERISTIC_UUID = 0x00,0x00,0x04,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t DIFFERENCE_CHARACTERISTIC_UUID = 0x00,0x00,0x08,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t POSITION_CHARACTERISTIC_UUID = 0x00,0x00,0x10,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
    //uint16_t SUNPANEL_CHARACTERISTIC_UUID = 0x00,0x00,0x20,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;    
    
    if (params->role == Gap::CENTRAL)
    {
        BLE &ble = BLE::Instance();
        ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
        // Discover all SERVICES and CHARACTERISTICS
        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback);
        
        // Works only for one characteristic, if you need to discover all characteristic, use the above command
        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, CONTROL_SERVICE_UUID, COMMAND_CHARACTERISTIC_UUID); 
        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, STATUS_CHARACTERISTIC_UUID);  
        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, DIFFERENCE_CHARACTERISTIC_UUID);
        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, POSITION_CHARACTERISTIC_UUID);
        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, SUNPANEL_CHARACTERISTIC_UUID);
    }
    
    printf("Remote Connected\n\r");
    
    myled = 1; // Power ON Led Green on CLIENT Board to indicate Connection between CLIENT and SERVER
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    (void)params;
    
    printf("Remote Disconnected\n\r");
    
    myled = 0; // Power OFF Led Green on CLIENT Board to indicate Disconnection between CLIENT and SERVER
}

/** 
 * Callback triggered when the ble initialization process has finished 
 */ 
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) 
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE)
    {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) { return; }

    // Set BT Address
    ble.gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);

    ble.gap().onConnection(connectionCallback);
    ble.onDisconnection(disconnectionCallback);
       
    ble.gattClient().onDataRead(onDataReadCallback);
    ble.gattClient().onDataWrite(myonDataWriteCallback);

    ble.gap().setScanParams(SCAN_INT, SCAN_WIND);
    ble.gap().startScan(advertisementCallback);
    
    // Main Loop
    while (true)
    {  
    
        //if (!ble.gattClient().isServiceDiscoveryActive())
        if (trigger_Op!=IDLE) // Wait until discoveryTerminationCallback
        {   
            
            // At present ARM BLE_API don't support Enable/Disable Notify from Client
            //command_bleCharacteristic.notify(); 
            //difference_bleCharacteristic.notify();
            //position_bleCharacteristic.notify();
            //sunpanel_bleCharacteristic.notify();
            
            // Don't run more ble .read or .write in the same loop
            // because the callback is an interrupt and it doesn't work correctly if overloaded
            
            if (Status == 0)
            { if (trigger_Op == READ_DIF) trigger_Op = WRITE_STA; } // if Display 0, skip READ_DIF,READ_POS,READ_SUN
            
            if (Status == 0 || Status == 1 || Status == 2)
            { if (trigger_Op == WRITE_DIF) trigger_Op = READ_STA; } // if Display 0 or 1 or 2, skip WRITE_DIF
            
            if (Status == 3)
            { if (trigger_Op == READ_DIF) trigger_Op = READ_POS; } // if Display 3, skip READ_DIF
            
            if (trigger_button == true) { trigger_Op = WRITE_STA; } // jump directly to WRITE_STA
              
            switch(trigger_Op)
            {
            case READ_STA:
                status_bleCharacteristic.read();
                trigger_Op = IDLE;
                break;
            case READ_DIF:
                difference_bleCharacteristic.read();
                trigger_Op = IDLE;
                break;
            case READ_POS:
                position_bleCharacteristic.read();
                trigger_Op = IDLE;
                break;
            case READ_SUN:
                sunpanel_bleCharacteristic.read();
                trigger_Op = IDLE;
                break;
            case WRITE_STA:
                if (trigger_button == true)
                {                         
                    memset (value_write, 0, COMMAND_DATA_LEN);
                    //value_write = Feature (4) + Type (1) + Data (2)
                    value_write[0] = (FeatureStatus & 0xff000000) >> 24;
                    value_write[1] = (FeatureStatus & 0x00ff0000) >> 16;
                    value_write[2] = (FeatureStatus & 0x0000ff00) >> 8;
                    value_write[3] = (FeatureStatus & 0x000000ff);
                    value_write[4] = (0x00);
                    value_write[5] = (Status_to_Write & 0xff00) >> 8;
                    value_write[6] = (Status_to_Write & 0x00ff);
                    command_bleCharacteristic.write(COMMAND_DATA_LEN, value_write);
                             
                    trigger_Op = IDLE;
                    trigger_button = false;
                } else {
                    #ifdef Sensors
                    trigger_Op = WRITE_DIF;
                    #else
                    trigger_Op = READ_STA;
                    #endif
                }
                break;                                 
            case WRITE_DIF:    
                #ifdef Sensors             
                accelerometer->Get_X_Axes(acc_data);
                acc_diff = acc_data[0];
                //printf("Send BLE Difference %d lux/mems\n\r", acc_diff);
                               
                memset (value_write, 0, COMMAND_DATA_LEN); 
                //value_write = Feature (4) + Type (1) + Data (2)
                value_write[0] = (FeatureDifference & 0xff000000) >> 24;
                value_write[1] = (FeatureDifference & 0x00ff0000) >> 16;
                value_write[2] = (FeatureDifference & 0x0000ff00) >> 8;
                value_write[3] = (FeatureDifference & 0x000000ff);
                value_write[4] = (0x00);
                value_write[5] = (acc_diff & 0xff00) >> 8;
                value_write[6] = (acc_diff & 0x00ff);
                command_bleCharacteristic.write(COMMAND_DATA_LEN, value_write);

                trigger_Op = IDLE;
                #else
                trigger_Op = READ_STA;
                #endif
                break;
            default:
                break;
            }
            
        }
        ble.waitForEvent();
    } 
}

/* Ticker --------------------------------------------------------------------*/
/*
void ticker_Callback(void)
{

}
*/
/* Main ----------------------------------------------------------------------*/

int main(void)
{
    printf("\r\n\r\nSunTracker Remote Control by Fabio Brembilla\r\n\r\n");
    
    #ifdef Sensors
    // Initializing Sensors Component
    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
    accelerometer = mems->GetAccelerometer();
    printf("Init Sensors OK\r\n");
    #endif
        
    //Ticker ticker;
    //ticker.attach(ticker_Callback, 1); // every 1 second
    
    mybutton.fall(&User_Button_Pressed);
    
    BLE::Instance().init(bleInitComplete);  
    
}