File content as of revision 24:b374c444d8ad:

/**
  ******************************************************************************
  * @file    main.cpp
  * @author  Central Labs / AST
  * @version V0.9.0
  * @date    23-Dec-2015
  * @brief   Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT(c) 2015 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.
  *
  ******************************************************************************
  */

#include "mbed.h"
#include "main.h"   // contains the condt compiling configuration
#include <cmath>
#include <math.h>
#include "Gap.h"
#include "debug.h"
#include "btle.h"
#include "blecommon.h"
#include "BLE.h"
#include "UUID.h"
#include "MotionFX_Manager.h"
#include "InterruptManager.h"
#include "DevI2C.h"
#include "BlueNRGGattServer.h"
#include "GattCharacteristic.h"
#include "LPS22HBSensor.h"
#include "LSM6DSLSensor.h"
#include "LSM303AGRMagSensor.h"
#include "LSM303AGRAccSensor.h"


typedef struct {
    int32_t AXIS_X;
    int32_t AXIS_Y;
    int32_t AXIS_Z;
} AxesRaw_TypeDef;

#ifdef CUST_CONS_SERV
#include "CustomConsoleService.h"
#endif

#ifdef CUST_BATT_SERV
#include "CustomBatteryService.h"
#endif

#ifdef CUST_SENS_SERV
#include "CustomSensorsService.h"
#define SPI_TYPE_LPS22HB   LPS22HBSensor::SPI3W
#define SPI_TYPE_LSM6DSL   LSM6DSLSensor::SPI3W
//DevI2C i2c(I2C_SDA, I2C_SCL);
//HTS221Sensor ht_sensor(&i2c);      // sensor on the mini cradle only (not on the tile)
SPI sens_intf(PB_15, NC, PB_13); // 3W mosi, sclk on Nucleo L476 same as BTLE
//LPS22HBSensor pt_sensor(&sens_intf, PA_3);	 // on SensorTile L476JG
static LPS22HBSensor pt_sensor(&sens_intf, PA_3, NC, SPI_TYPE_LPS22HB); 
//LSM6DSLSensor acc_gyro(&sens_intf, PB_12, NC, PA_2 );	 // on SensorTile L476JG  
static LSM6DSLSensor acc_gyro(&sens_intf,PB_12, NC, PA_2, SPI_TYPE_LSM6DSL); 
static LSM303AGRMagSensor magnetometer(&sens_intf, PB_1 ); //on SensorTile L476JG				  
static LSM303AGRAccSensor accelerometer(&sens_intf, PC_4 ); //on SensorTile L476JG	
#endif

#ifdef CUST_CONFIG_SERV
#include "CustomConfigService.h"
//static unsigned char    isCal = 0;
//static bool             ForceReCalibration =0;
#endif

static unsigned char    isCal = 0;
static bool             ForceReCalibration =0;

#ifdef CUST_SW_SERV
#include "CustomSoftwareService.h"
#endif

static bool             isBlueButtonSequence = false;
static                  Ticker BlueButtonTimer;
//static DigitalOut       greenled(LED1);
//static int8_t           txPower=-3;
static uint32_t         TimeStamp=0;
static int              BLEConnectionStatus =0;
static int              HowManyButtonPress =0;
static BLE              * p_BLEdev = NULL;
static bool             SensorFusionOK = false;
static int              ble_started=0;

static Queue <char, 8> QueueMainActivity;
static EventQueue eventQueue(/* event count */ 16 * /* event size */ 32);

#define BLE_STARTED       5
    
#ifdef CUST_CONS_SERV
static CustomConsoleService    * p_customconsoleservice   = NULL;
#endif

#ifdef CUST_SENS_SERV
static CustomSensorService     * p_customsensorservice    = NULL;
#endif  

#ifdef CUST_SW_SERV
static CustomSoftwareService   * p_customsoftwareservice  = NULL;
static MFX_input_t               MotionFX_Engine_In;
static MFX_input_t             * p_MotionFX_Engine_In = &MotionFX_Engine_In; 
static MFX_output_t              MotionFX_Engine_Out;
static MFX_output_t            * p_MotionFX_Engine_Out = &MotionFX_Engine_Out;
#endif
static osxMFX_calibFactor        magOffset;


#ifdef CUST_CONFIG_SERV
static CustomConfigService     * p_customconfigservice    = NULL;
#endif


/***************************    Calibration functions    **************************/

/**
 * @brief  Check if there are a valid Calibration Values in Memory and read them
 * @param None
 * @retval unsigned char Success/Not Success
 */
static unsigned char ReCallCalibrationFromMemory(void)
#ifdef BLUEMSYS_STORE_CALIB_FLASH
{
    /* ReLoad the Calibration Values from FLASH */
    unsigned char Success=1;
    uint32_t Address = BLUEMSYS_FLASH_ADD;
    __IO uint32_t data32 = *(__IO uint32_t*) Address;
    if(data32== BLUEMSYS_CHECK_CALIBRATION) {
        int32_t ReadIndex;
        float ReadmagOffset[7];

        for(ReadIndex=0; ReadIndex<7; ReadIndex++) {
            Address +=4;
            data32 = *(__IO uint32_t*) Address;
            ReadmagOffset[ReadIndex]=data32;
        }
        isCal=1;
        magOffset.magOffX    = (signed short) ReadmagOffset[0];
        magOffset.magOffY    = (signed short) ReadmagOffset[1];
        magOffset.magOffZ    = (signed short) ReadmagOffset[2];
#ifdef BLUEMSYS_DEBUG_CALIB
        /* Debug */
        printf("magOffX   ->%d\r\n",magOffset.magOffX);
        printf("magOffY   ->%d\r\n",magOffset.magOffY);
        printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
#endif /* BLUEMSYS_DEBUG_CALIB */
        printf("Calibration Read from Flash\r\n");
    } else {
        printf("Calibration Not present in FLASH\r\n");
        magOffset.magOffX    = 0;
        magOffset.magOffY    = 0;	
        magOffset.magOffZ    = 0;				
        isCal=0;
    }
    return Success;
}
#else /* BLUEMSYS_STORE_CALIB_FLASH */
{
	unsigned char Success= 1;			
    magOffset.magOffX    = 0;
    magOffset.magOffY    = 0;	
    magOffset.magOffZ    = 0;	
    return Success;
}
#endif /* BLUEMSYS_STORE_CALIB_FLASH */

static unsigned char ResetCalibrationInMemory(void)
#ifdef BLUEMSYS_STORE_CALIB_FLASH
{
    /* Reset Calibration Values in FLASH */
    unsigned char Success=1;

    /* Erase First Flash sector */
    FLASH_EraseInitTypeDef EraseInitStruct;
    uint32_t SectorError = 0;
    EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
    EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3;
    EraseInitStruct.Sector = BLUEMSYS_FLASH_SECTOR;
    EraseInitStruct.NbSectors = 1;

    /* Unlock the Flash to enable the flash control register access *************/
    HAL_FLASH_Unlock();

    if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
        /*
          Error occurred while sector erase.
          User can add here some code to deal with this error.
          SectorError will contain the faulty sector and then to know the code error on this sector,
          user can call function 'HAL_FLASH_GetError()'
        */
        /*
          FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
        */

//   Error_Handler();
        Success=0;
    } else {
        printf("Erase calibration in FLASH Memory\r\n");
    }

    /* Lock the Flash to disable the flash control register access (recommended
    to protect the FLASH memory against possible unwanted operation) *********/
    HAL_FLASH_Lock();
    magOffset.magOffX    = 0;
    magOffset.magOffY    = 0;	
    magOffset.magOffZ    = 0;	
		
    return Success;
}
#else /* BLUEMSYS_STORE_CALIB_FLASH */
{
	  unsigned char Success=1;			
    magOffset.magOffX    = 0;
    magOffset.magOffY    = 0;	
    magOffset.magOffZ    = 0;	
    return Success;
}
#endif /* BLUEMSYS_STORE_CALIB_FLASH */

/**
 * @brief  Save the Magnetometer Calibration Values to Memory
 * @param None
 * @retval unsigned char Success/Not Success
 */
static unsigned char SaveCalibrationToMemory(void)
{
    unsigned char Success=1;

    /* Reset Before The data in Memory */
    Success = ResetCalibrationInMemory();

    if(Success)
#ifdef BLUEMSYS_STORE_CALIB_FLASH
    {
        /* Store in Flash Memory */
        uint32_t Address = BLUEMSYS_FLASH_ADD;
        uint32_t WritemagOffset[8];
        int32_t WriteIndex;
        WritemagOffset[0] = BLUEMSYS_CHECK_CALIBRATION;
        WritemagOffset[1] = (uint32_t) magOffset.magOffX;
        WritemagOffset[2] = (uint32_t) magOffset.magOffY;
        WritemagOffset[3] = (uint32_t) magOffset.magOffZ;

        /* Unlock the Flash to enable the flash control register access *************/
        HAL_FLASH_Unlock();

        for(WriteIndex=0; WriteIndex<8; WriteIndex++) {
            if (HAL_FLASH_Program(TYPEPROGRAM_WORD, Address,WritemagOffset[WriteIndex]) == HAL_OK) {
                Address = Address + 4;
            } else {
                /* Error occurred while writing data in Flash memory.
                   User can add here some code to deal with this error */
                /*
                  FLASH_ErrorTypeDef errorcode = HAL_FLASH_GetError();
                */
//       Error_Handler();
            }
        }
#ifdef BLUEMSYS_DEBUG_CALIB
        /* Debug */
        printf("magOffX   ->%d\r\n",magOffset.magOffX);
        printf("magOffY   ->%d\r\n",magOffset.magOffY);
        printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
#endif /* BLUEMSYS_DEBUG_CALIB */

        printf("New Calibration Values Saved in FLASH\r\n");

        /* Lock the Flash to disable the flash control register access (recommended
         to protect the FLASH memory against possible unwanted operation) *********/
        HAL_FLASH_Lock();
    }
#else /* BLUEMSYS_STORE_CALIB_FLASH */
    {
#ifdef BLUEMSYS_DEBUG_CALIB
        /* Debug */
        printf("magOffX   ->%d\r\n",magOffset.magOffX);
        printf("magOffY   ->%d\r\n",magOffset.magOffY);
        printf("magOffZ   ->%d\r\n",magOffset.magOffZ);
#endif /* BLUEMSYS_DEBUG_CALIB */
    }
#endif /* BLUEMSYS_STORE_CALIB_FLASH */

    return Success;
}

/***************************    End Calibration functions    **************************/

static void floatToInt(float in, int32_t *out_int, int32_t *out_dec, int32_t dec_prec)
{
    *out_int = (int32_t)in;
    in = in - (float)(*out_int);
    *out_dec = (int32_t)(float)(int)(in*pow((float)10,(int)dec_prec));
}

static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
{
    PRINTF("onUpdatesEnabledCallback! handle: %x\n\r", handle);
#ifdef CUST_SENS_SERV
    if (p_customsensorservice)
        p_customsensorservice->enNotify (handle);
#endif

#ifdef CUST_SW_SERV
    if (p_customsoftwareservice)
        p_customsoftwareservice->enNotify (handle);
#endif

#ifdef CUST_CONFIG_SERV
    if (p_customconfigservice) {
        p_customconfigservice->enNotify (handle);
        p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
        p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
    }
#endif
		
#ifdef CUST_CONS_SERV
    if (p_customconsoleservice)
        p_customconsoleservice->enNotify (handle);
#endif
}

static void onUpdatesDisabledCallback(Gap::Handle_t handle)
{
    PRINTF("onUpdatesDisabledCallback! handle: %x\n\r", handle);
#ifdef CUST_SENS_SERV
    if (p_customsensorservice)
        p_customsensorservice->disNotify (handle);
#endif

#ifdef CUST_SW_SERV
    if (p_customsoftwareservice)
        p_customsoftwareservice->disNotify (handle);
#endif

#ifdef CUST_CONFIG_SERV
    if (p_customconfigservice)
        p_customconfigservice->disNotify (handle);
#endif
		
#ifdef CUST_CONS_SERV
    if (p_customconsoleservice)
        p_customconsoleservice->disNotify (handle);
#endif
}

static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
{
    printf("Disconnected\n\r");
    printf("Restarting the advertising process\n\r");
    TimeStamp =0;
    BLEConnectionStatus =0;
#ifdef CUST_SENS_SERV
    if (p_customsensorservice)
        p_customsensorservice->updateConnectionStatus(DISCONNECTED);
#endif
#ifdef CUST_SW_SERV
    if (p_customsoftwareservice)
        p_customsoftwareservice->updateConnectionStatus(DISCONNECTED);
#endif
#ifdef CUST_CONFIG_SERV
    if (p_customconfigservice)
        p_customconfigservice->updateConnectionStatus(DISCONNECTED);
#endif
#ifdef CUST_CONS_SERV
    if (p_customconsoleservice)
        p_customconsoleservice->updateConnectionStatus(DISCONNECTED);
#endif
    if (p_BLEdev)
        p_BLEdev->startAdvertising();
}

static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
{
    printf("\rConnected: %x", connectionParams->peerAddr[Gap::ADDR_LEN-1]);
    for(int i=Gap::ADDR_LEN-2; i>=0; i--) printf(":%x", connectionParams->peerAddr[i]);
    printf("\n\r");
            
    TimeStamp =0;
    BLEConnectionStatus =1;
#ifdef CUST_SENS_SERV
    if (p_customsensorservice)
        p_customsensorservice->updateConnectionStatus(CONNECTED);
#endif
#ifdef CUST_SW_SERV
    if (p_customsoftwareservice)
        p_customsoftwareservice->updateConnectionStatus(CONNECTED);
#endif
#ifdef CUST_CONFIG_SERV
    if (p_customconfigservice)
        p_customconfigservice->updateConnectionStatus(CONNECTED);
#endif
#ifdef CUST_CONS_SERV
    if (p_customconsoleservice)
        p_customconsoleservice->updateConnectionStatus(CONNECTED);
#endif
}

static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
{
    float temp, hum, pres;
    int16_t TempToSend, HumToSend;
    uint32_t    PresToSend;
    int32_t decPart, intPart;
    AxesRaw_TypeDef Magn, Acc, Gyro;
    Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
#ifdef CUST_SENS_SERV
#ifdef DISAGGREGATED_ENV_CHAR
    if (p_customsensorservice->isTempHandle(handle)) {
        pt_sensor.get_temperature(&temp);
        floatToInt(temp, &intPart, &decPart, 1);
        TempToSend = intPart*10+decPart;
        p_customsensorservice->sendEnvTemperature(TempToSend, TimeStamp);

    } else if (p_customsensorservice->isHumHandle(handle)) {
//        ht_sensor.get_humidity(&hum);
        floatToInt(hum, &intPart, &decPart, 1);
        HumToSend = intPart*10+decPart;
        p_customsensorservice->sendEnvHumidity(HumToSend, TimeStamp);

    } else if (p_customsensorservice->isPresHandle(handle)) {
        pt_sensor.get_pressure(&pres);
        floatToInt(pres, &intPart, &decPart, 1);
        PresToSend = intPart*100+decPart;
        p_customsensorservice->sendEnvPressure(PresToSend, TimeStamp);

    } else
#endif        
        if (p_customsensorservice->isGyroHandle(handle)) {
        acc_gyro.get_g_axes((int32_t *)&Gyro);
        p_customsensorservice->sendEnvGyroscope (&Gyro, TimeStamp);

    } else if (p_customsensorservice->isAccHandle(handle)) {
        accelerometer.get_x_axes((int32_t *)&Acc);
        p_customsensorservice->sendEnvAccelerometer (&Acc, TimeStamp);

    } else if (p_customsensorservice->isMagHandle(handle)) {
        magnetometer.get_m_axes((int32_t *)&Magn);
        p_customsensorservice->sendEnvMagnetometer(&Magn, TimeStamp, &magOffset);

    } else if (p_customsensorservice->isAccGyroMagHandle(handle)) {
        p_customsensorservice->sendEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);
        
    } else if (p_customsensorservice->isPresHumTempHandle(handle)) {
        pt_sensor.get_temperature(&temp);
        floatToInt(temp, &intPart, &decPart, 1);
        TempToSend = intPart*10+decPart;
//        ht_sensor.get_humidity(&hum);
        floatToInt(hum, &intPart, &decPart, 1);
        HumToSend = intPart*10+decPart;
        pt_sensor.get_pressure(&pres);
        floatToInt(pres, &intPart, &decPart, 1);
        PresToSend = intPart*100+decPart;        
        p_customsensorservice->sendEnvPresHumTemp (PresToSend, HumToSend, TempToSend, TimeStamp);
    }
#endif

#ifdef CUST_SW_SERV
    if (p_customsoftwareservice->isQuatHandle(handle)) {
//          p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);   // dont need to update because already calculated/update into main loop

    } else if (p_customsoftwareservice->isFloatQuatHandle(handle)) {
//          p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);  // dont need to update because already calculated/update into main loop
    }
#endif

#ifdef CUST_CONFIG_SERV
    if (p_customconfigservice->isConfHandle(handle)) {
        p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
        p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
    }
#endif
}

static void onDataWriteCallback(const GattWriteCallbackParams *eventDataP)
{
    Gap::Handle_t handle = eventDataP->handle - BLE_HANDLE_VALUE_OFFSET;
    PRINTF("onDataWriteCallback!!\n\r");

#ifdef CUST_CONFIG_SERV
            if (p_customconfigservice->isConfHandle(handle)) {
                /* Received one write command from Client on calibration characteristc */
                uint32_t FeatureMask = (eventDataP->data[3]) | (eventDataP->data[2]<<8) | (eventDataP->data[1]<<16) | (eventDataP->data[0]<<24);
                uint8_t Command = eventDataP->data[4];                
                switch (Command) {
                    case W2ST_COMMAND_CAL_STATUS:

                        /* Replay with the calibration status for the feature */
                        //  Config_Notify(FeatureMask,Command,isCal);
                        p_customconfigservice->sendConfigState(FeatureMask, Command, isCal, TimeStamp);
                        break;
                    case W2ST_COMMAND_CAL_RESET:
                        /* Reset the calibration */
                        ForceReCalibration=1;
                        break;
                    case W2ST_COMMAND_CAL_STOP:

                        /* Do nothing in this case */
                        break;
                    default:
//        if(StdErrNotification){
//          BytesToWrite = sprintf((char *)BufferToWrite, "Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
//          Stderr_Update(BufferToWrite,BytesToWrite);
//        } else {
//          printf("Calibration UNKNOW Signal For Features=%x\n\r",FeatureMask);
//        }
                        break;
                }

            }
#endif
}

static void onDataSentCallback(unsigned count)
{
    PRINTF("onDataSentCallback!!\n\r");
}


static void onTimeoutCallback(Gap::TimeoutSource_t source)
{
    PRINTF("onTimeoutCallback!!\n\r");
}

static void onConfirmationReceivedCallback(uint16_t attributeHandle)
{
    PRINTF("onConfirmationReceivedCallback!!\n\r");
}


static void Ticker_BlueButton(void)
{
#ifdef CUST_CONS_SERV   
static uint8_t          BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
static uint8_t          BytesToWrite;   
#endif
    
        BlueButtonTimer.detach();   
//        printf (" butt time expired \n\r"); 
#ifdef CUST_CONS_SERV
    BytesToWrite = sprintf((char *)BufferToWrite, "nr%d push in %1.1fs\r\n",HowManyButtonPress, BUTTON_TIME);
    p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
#endif      
        isBlueButtonSequence = false;       
        HowManyButtonPress =0;  
}

/**
 * CB Triggered by the user blue button press;
 */
static void BlueButtonPressed ()
{
#ifdef CUST_CONS_SERV   
static uint8_t          BufferToWrite[W2ST_CONSOLE_MAX_CHAR_LEN];
static uint8_t          BytesToWrite;   
#endif	
//        printf (" CB  BlueButtonPressed  PRESSED!!!!!!!!! %d\n\r", HowManyButtonPress);
    if (HowManyButtonPress == 0) { // first push
        BlueButtonTimer.attach(&Ticker_BlueButton, BUTTON_TIME);            
                HowManyButtonPress++;           
                isBlueButtonSequence = false;           
    } else {
                HowManyButtonPress++;           
                if (HowManyButtonPress == BLUEMSYS_N_BUTTON_PRESS ) {
                        BlueButtonTimer.detach();                   
//                        printf (" CB  BlueButtoon SEQUENCE!!!!!!!!! \n\r");
                        HowManyButtonPress =0;
                        if (BLEConnectionStatus) isBlueButtonSequence = true;  
#ifdef CUST_CONS_SERV
                        BytesToWrite = sprintf((char *)BufferToWrite, "BlueButton SEQ\r\n");
                        p_customconsoleservice->updateTerm(BufferToWrite,BytesToWrite);
#endif    									
                }       
        }
}


void onBleInitError(BLE &ble, ble_error_t error)
{
    (void)ble;
    (void)error;
	printf ("--->> onBleInitError\n\r");
   /* Initialization error handling should go here */
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;
    if (error != BLE_ERROR_NONE) {
        onBleInitError(ble, error);
        printf ("--->> bleInitComplete ERROR\n\r");			
        return;
    }
    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }
	char msg = BLE_STARTED;
	QueueMainActivity.put((char*)msg);			
}

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


void updateSensorValue (void)
{
	  float temp, hum, pres;	
    AxesRaw_TypeDef Magn, Acc, Gyro;
    int32_t decPart, intPart;
    int16_t TempToSend, HumToSend;
    uint32_t    PresToSend;
#ifdef CUST_SW_SERV    
    static AxesRaw_TypeDef quat_axes[SEND_N_QUATERNIONS];
    static float QuatFloat[SEND_N_QUATERNIONS];
    static    uint32_t QuaternionNumber =0;
    static    uint32_t CounterFloat =0;	
#endif    	
    static unsigned int envTime =0;	
    static unsigned int CounterAGM =0;	
	
	      TimeStamp++;	
#ifdef USE_SENSOR_FUSION_LIB	
	    static unsigned int calibTime =0;	
	      // Run Calibration from command 
        if (BLEConnectionStatus) {
            if ((isBlueButtonSequence || ForceReCalibration) && SensorFusionOK ) {
                printf("ForceReCalibration\r\n");
                /* Reset the Compass Calibration */
                isCal=0;
                calibTime =0;
                ForceReCalibration =0;
                isBlueButtonSequence = false;                
//                greenled =0;							
                /* Notifications of Compass Calibration */
#ifdef CUST_CONFIG_SERV
                p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
                p_customconfigservice->sendConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
#endif
                /* Reset the Calibration */
                ResetCalibrationInMemory();
							  MotionFX_manager_MagCal_start(SAMPLE_PERIOD);
            }
        }	
#endif
				
        // Update MEMS @ 100Hz and calculate quaternions
        if (BLEConnectionStatus) {				
#ifdef CUST_SENS_SERV
            int32_t err=0; 
            err = acc_gyro.get_x_axes((int32_t*)&Acc);
            if (err != 0) {
                printf ("= * ERROR %d GetAccelerometer\n\r", (int)err);                          
            }
						
            err = acc_gyro.get_g_axes((int32_t *)&Gyro);
            if (err != 0) {
                printf ("= * ERROR %d GetGyroscope\n\r", (int)err);                          
            }   
						
            err = magnetometer.get_m_axes((int32_t *)&Magn);
            if (err != 0) {
                printf ("= * ERROR %d Get_M_Axes\n\r", (int)err);                            
            }    
            if(CounterAGM >= ACC_GYRO_MAG_UPDATE_MUL_10MS) {	        
                CounterAGM=0;    								
                p_customsensorservice->updateEnvAccelerometer (&Acc, TimeStamp);
                p_customsensorservice->updateEnvGyroscope(&Gyro, TimeStamp);
                p_customsensorservice->updateEnvMagnetometer(&Magn, TimeStamp, &magOffset);
                p_customsensorservice->updateEnvAccGyroMag (&Acc, &Gyro, &Magn, TimeStamp, &magOffset);                            
            } else { 
						    CounterAGM++; 
						}
#endif	
						
#ifdef USE_SENSOR_FUSION_LIB
            if (SensorFusionOK) {
                MotionFX_Engine_In.gyro[0] = Gyro.AXIS_X  * FROM_MDPS_TO_DPS;
                MotionFX_Engine_In.gyro[1] = Gyro.AXIS_Y  * FROM_MDPS_TO_DPS;
                MotionFX_Engine_In.gyro[2] = Gyro.AXIS_Z  * FROM_MDPS_TO_DPS;

                MotionFX_Engine_In.acc[0] = Acc.AXIS_X * FROM_MG_TO_G;
                MotionFX_Engine_In.acc[1] = Acc.AXIS_Y * FROM_MG_TO_G;
                MotionFX_Engine_In.acc[2] = Acc.AXIS_Z * FROM_MG_TO_G;

                MotionFX_Engine_In.mag[0] = Magn.AXIS_X * FROM_MGAUSS_TO_UT50;
                MotionFX_Engine_In.mag[1] = Magn.AXIS_Y * FROM_MGAUSS_TO_UT50;
                MotionFX_Engine_In.mag[2] = Magn.AXIS_Z * FROM_MGAUSS_TO_UT50;   

                MotionFX_manager_run(p_MotionFX_Engine_In, p_MotionFX_Engine_Out,  MOTIONFX_ENGINE_DELTATIME);
            }						
#endif						
#ifdef CUST_SW_SERV 
            if (SensorFusionOK) {
                /* Update quaternions (compact)*/
                if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
                    quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * (-10000));
                    quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * (-10000));
                    quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * (-10000));
                } else {
                    quat_axes[QuaternionNumber].AXIS_X = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[0] * 10000);
                    quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[1] * 10000);
                    quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(p_MotionFX_Engine_Out->quaternion_9X[2] * 10000);
                }

                if (QuaternionNumber >= SEND_N_QUATERNIONS-1) {
                    p_customsoftwareservice->updateQuaternions(quat_axes, TimeStamp);                    
                    QuaternionNumber =0;
                } else {
                    QuaternionNumber++;
                }

                /* Update Float Quaternions */
                /* Every QUAT_FLOAT_UPDATE_MUL_10MS*10 mSeconds Send Float Quaternion informations via bluetooth */
                if(CounterFloat>=QUAT_FLOAT_UPDATE_MUL_10MS) {
                    if(p_MotionFX_Engine_Out->quaternion_9X[3] < 0) {
                        QuatFloat[0] = - p_MotionFX_Engine_Out->quaternion_9X[0];
                        QuatFloat[1] = - p_MotionFX_Engine_Out->quaternion_9X[1];
                        QuatFloat[2] = - p_MotionFX_Engine_Out->quaternion_9X[2];
                    } else {
                        QuatFloat[0] =   p_MotionFX_Engine_Out->quaternion_9X[0];
                        QuatFloat[1] =   p_MotionFX_Engine_Out->quaternion_9X[1];
                        QuatFloat[2] =   p_MotionFX_Engine_Out->quaternion_9X[2];
                    }
                    p_customsoftwareservice->updateFloatQuaternions(QuatFloat, TimeStamp);
                    CounterFloat=0;
                } else  {
                    CounterFloat++;
                }
            }
#endif						
				}			
#ifdef CUST_CONFIG_SERV				
				MFX_MagCal_input_t   mag_data_in;
				MFX_MagCal_output_t  cal_data_out;
				// Check if is calibrated  @ 25Hz
				calibTime = calibTime + MEMS_TIMER;
        if(isCal!=0x01 && calibTime>= CALIB_TIMER && BLEConnectionStatus && SensorFusionOK ) {
				calibTime = 0;
            /* Run Compass Calibration @ 25Hz */
            mag_data_in.mag[0]= MotionFX_Engine_In.mag[0];
            mag_data_in.mag[1]= MotionFX_Engine_In.mag[1];
            mag_data_in.mag[2]= MotionFX_Engine_In.mag[2];
            mag_data_in.time_stamp = TimeStamp;
            MotionFX_manager_MagCal_run(&mag_data_in, &cal_data_out);
					
            /* Control the calibration status */
            if( (cal_data_out.cal_quality == MFX_MAGCALOK) || (cal_data_out.cal_quality == MFX_MAGCALGOOD) )
            {
                magOffset.magOffX = (int32_t)(cal_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS);
                magOffset.magOffY = (int32_t)(cal_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS);
                magOffset.magOffZ = (int32_t)(cal_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS);
        
                /* Disable magnetometer calibration */
                MotionFX_manager_MagCal_stop(SAMPLE_PERIOD);
							
                isCal= 1;							
//				greenled = 1;
                printf("Compass Calibrated\n\r");					
                
							  SaveCalibrationToMemory();
							
                /* Notifications of Compass Calibration */
                p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_SHORT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);
                p_customconfigservice->updateConfigState(MASK_CALIB_SENSORFUSION_FLOAT, W2ST_COMMAND_CAL_STATUS, isCal, TimeStamp);							
            }
        }
#endif									
        // Update environmental sensors @ 2Hz						
        if (BLEConnectionStatus) {
          envTime = envTime + MEMS_TIMER;					
					if ( envTime >= ENV_TIMER) {
						envTime =0;
            int32_t err;                                            
#ifdef CUST_SENS_SERV
#ifdef DISAGGREGATED_ENV_CHAR
            if (p_customsensorservice->isTempNotificationEn()) {
                err = pt_sensor.get_temperature(&temp);
                if ( err != 0) {
                    printf ("= * ERROR %d GetTemperature\n\r", (int)err);
                } else {
                    floatToInt(temp, &intPart, &decPart, 1);													
                    TempToSend = intPart*10+decPart;
                    p_customsensorservice->updateEnvTemperature(TempToSend, TimeStamp);
                }
            }

            if (p_customsensorservice->isHumNotificationEn()) {
//                err = ht_sensor.get_humidity(&hum);  
                if ( err != 0) {
                    printf ("= * ERROR %d GetHumidity\n\r", (int)err);                           
                } else {
                    floatToInt(hum, &intPart, &decPart, 1);
                    HumToSend = intPart*10+decPart;
                    p_customsensorservice->updateEnvHumidity(HumToSend, TimeStamp);       
                }
            }

            if (p_customsensorservice->isPresNotificationEn()) {
                err = pt_sensor.get_pressure(&pres);
                if ( err != 0) {
                    printf ("= * ERROR GetPressure\n\r");                           
                } else {
                    floatToInt(pres, &intPart, &decPart, 1);
                    PresToSend = intPart*100+decPart;
                    p_customsensorservice->updateEnvPressure(PresToSend, TimeStamp);
                }
            }
#endif            
            if (p_customsensorservice->isPresHumTempNotificationEn()) {
                err = pt_sensor.get_pressure(&pres);
                if ( err != 0) {
                    printf ("= * ERROR GetPressure\n\r");                           
                } else {
                    floatToInt(pres, &intPart, &decPart, 1);
                    PresToSend = intPart*100+decPart;
                }
//                err = ht_sensor.get_humidity(&hum);  
                if ( err != 0) {
                    printf ("= * ERROR %d GetHumidity\n\r", (int)err);                           
                } else {
                    floatToInt(hum, &intPart, &decPart, 1);
                    HumToSend = intPart*10+decPart;
                }                
                err = pt_sensor.get_temperature(&temp);
                if ( err != 0) {
                    printf ("= * ERROR %d GetTemperature\n\r", (int)err);
                } else {
                    floatToInt(temp, &intPart, &decPart, 1);													
                    TempToSend = intPart*10+decPart;
                }                
                p_customsensorservice->updateEnvPresHumTemp(PresToSend, HumToSend, TempToSend, TimeStamp);                
            }            
#endif
        }
    }							
}

void periodicCallback(void)
{
    eventQueue.call(updateSensorValue);
}

    
int main()
{
    bool isgyro_lsm6ds0Present=false;
    bool isgyro_lsm6ds3Present=false;    
    TimeStamp =0;	

#ifdef CUST_SENS_SERV
//    if (p_mems_expansion_board->gyro_lsm6ds0) isgyro_lsm6ds0Present=true;
//    if (p_mems_expansion_board->gyro_lsm6ds3) isgyro_lsm6ds3Present=true;    
#endif
//    greenled = 0;
//    InterruptIn BlueButton(USER_BUTTON);    
//    BlueButton.fall(&BlueButtonPressed);
    
    printf("\r\nSTMicroelectronics BlueMicrosystem1 W2ST:\r\n"
#ifdef CUST_SENS_SERV
           "\tGyro lsmds0 present: %d, lsmds3 present: %d\n\r"
           "\tSend Every %dmS Temperature/Humidity/Pressure\r\n"
           "\tSend Every %dmS Acc/Gyro/Magneto\r\n\n"   
#endif  
#ifdef CUST_SW_SERV
           ,"\tSend Every %dmS %d Quaternions\r\n"
           "\tSend Every %dmS Float precision Quaternion\r\n"
#endif
#ifdef CUST_SENS_SERV
           ,isgyro_lsm6ds0Present,
           isgyro_lsm6ds3Present,
           ENV_TIMER,
           MEMS_TIMER*ACC_GYRO_MAG_UPDATE_MUL_10MS
#endif
#ifdef CUST_SW_SERV
           ,MEMS_TIMER*SEND_N_QUATERNIONS,SEND_N_QUATERNIONS,
           MEMS_TIMER*QUAT_FLOAT_UPDATE_MUL_10MS
#endif
          );


    /* Initialize MotionFX library */
#ifdef USE_SENSOR_FUSION_LIB    
//    bool DS3_OnBoard = DS3_ONBOARD;    
    MotionFX_manager_init ((unsigned char)LSM6DSL_G_0);
    MotionFX_manager_start_9X();
    SensorFusionOK = true;
    printf("SW      Service sensor fusion library added successfully\r\n");
#else 
    printf("sensor fusion lib disabled \n\r");    
    SensorFusionOK = false;     
#endif
                    
    if (NUCLEO_I2C_SHIELDS_SPEED < MAX_I2C_SPEED) 
        printf ("Warning I2C speed set to: %d (max: %d)\n\r", NUCLEO_I2C_SHIELDS_SPEED, MAX_I2C_SPEED);
 
	ble_started = 0;
    BLE &ble = BLE::Instance();
	p_BLEdev = &ble;
    p_BLEdev->onEventsToProcess(scheduleBleEventsProcessing);
    p_BLEdev->init(bleInitComplete);
 
    osEvent RxEvt = QueueMainActivity.get();	 // waiting for bleInitComplete cb to be called
	if (RxEvt.value.v == BLE_STARTED) ble_started = 1;
    
    /* set the BLE CB functions */
    p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
    p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
    p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
    p_BLEdev->gattServer().onDataSent(onDataSentCallback);
    p_BLEdev->gattServer().onDataRead(onDataReadCallback);
    p_BLEdev->gattServer().onDataWritten(onDataWriteCallback);
		
    p_BLEdev->gap().onConnection(onConnectionCallback);
    p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
    p_BLEdev->gap().onTimeout(onTimeoutCallback);    
         
#ifdef CUST_CONFIG_SERV
    p_customconfigservice = new CustomConfigService(*p_BLEdev);
    if (!p_customconfigservice) {
        printf("SW      Service W2ST calibration add FAILED!\n\r");
        return 0;  
    }
    printf("SW      Service W2ST calibration added successfully\r\n");
#endif
#ifdef CUST_SENS_SERV
    magnetometer.enable();
    accelerometer.enable();
    acc_gyro.enable_g();		
    acc_gyro.enable_x();		
//    ht_sensor.enable();
    pt_sensor.enable();		

    uint8_t id22=0, idLSM6=0, id303mag=0, id303acc=0, id221;
    pt_sensor.read_id(&id22);
    acc_gyro.read_id(&idLSM6);				
    magnetometer.read_id(&id303mag);
    accelerometer.read_id(&id303acc);
//    ht_sensor.read_id(&id221);

    printf("LS303acc ID %x LS303mag ID %x LSM6DSL ID %x LPS22HB ID %x \r\n", id303acc, id303mag, idLSM6, id22/*, id221*/);	  
    printf("\r\n");     
    
    p_customsensorservice = new CustomSensorService(*p_BLEdev);
    if (!p_customsensorservice) {
        printf("\n\rHW      Service W2ST sensors add FAILED!\n\r");
        return 0;
    }
    printf("\rHW      Service W2ST sensors added successfully\r\n");
#endif       
#ifdef CUST_SW_SERV
    if (SensorFusionOK) {
        p_customsoftwareservice = new CustomSoftwareService(*p_BLEdev);
        if (!p_customsoftwareservice) {
            printf("SW      Service W2ST quaternions add FAILED!\n\r");
            return 0;
        }
        printf("SW      Service W2ST quaternions added successfully\r\n");
    }
#endif      
#ifdef CUST_CONS_SERV
    p_customconsoleservice = new CustomConsoleService(*p_BLEdev);
    if (!p_customconsoleservice) {
        printf("\n\rHW      Service W2ST console add FAILED!\n\r");
        return 0;
    }
    printf("\rHW      Service W2ST console added successfully\r\n");
#endif
        
    const static char     DEVICE_NAME[]        = BLE_DEV_NAME;
    /* Setup advertising. */
    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
//    p_BLEdev->gap().accumulateAdvertisingPayloadTxPower(txPower); 
#ifdef USE_SENSOR_FUSION_LIB        
    uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x01,0x80};
#else
//    uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
      uint8_t dat[]= {0x01,0x80,0x00,0xFC,0x00,0x00};
#endif      
    p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
    
    printf("SERVER: BLE Stack Initialized\r\n");
    printf("Starting Advertising...\n\r");
    p_BLEdev->gap().startAdvertising();
	    
    /* Control if the calibration is already available in memory */
#ifdef CUST_CONFIG_SERV		
    if (SensorFusionOK) {
        ReCallCalibrationFromMemory();

        /* Switch on/off the LED according to calibration */
        if(isCal) {
//            greenled =1;
        } else {
//            greenled =0;
        }
    }		
#endif		
    eventQueue.call_every(MEMS_TIMER, periodicCallback); // Start the activity		
    eventQueue.dispatch_forever();
}