File content as of revision 25:00220cf2e628:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  Fabio Brembilla
 * @version V2.0.0
 * @date    May, 2016
 * @brief   SunTracker + BLE (Server) Vertical Application
 *          This application use IHM01A1, 6180XA1, 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 FlightSense //6180XA1 Main (remove only for debug)
#define EasySpin    //IHM01A1 Main (remove only for debug)
#define Sensors     //IKS01A1 Option
#define Ble         //IDB0XA1 Option
//#define Debug     //Verbose about Printf

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

// Mbed specific header files
#include "mbed.h"

// Helper header files
#include "DevSPI.h"
#include "DevI2C.h"

// Component specific header files
#ifdef FlightSense
    #include "x_nucleo_6180xa1.h"
#endif

#ifdef EasySpin
    #include "l6474_class.h"
#endif

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

// C header files
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

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

#ifdef Ble

#include "debug.h" // Need for PRINTF
#include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32

// 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];

typedef enum ConnectionStatus_t {
    DISCONNECTED    =0,
    CONNECTED       =1
} cns_t;

#define BLE_DEV_NAME "SunTracker"
#define BLE_DEV_MAC 0xFC,0x03,0x02,0x01,0xFB,0xFA // SERVER address (must be set into CLIENT BLE_peer_address_BE)
#define BLE_ADVERTISING_INTERVAL 100
uint8_t dat[] = {0x01,0x80,0x00,0x00,0x78,0x00}; // Data Sent during Advertising, must respect a Standard ST
    
#include "CustomControlService.h"
#include "CustomSensorsService.h"

static BLE *p_BLEdev = NULL;
static CustomControlService *p_customcontrolservice = NULL;
static CustomSensorService  *p_customsensorservice = NULL;

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

#endif

/* Definitions ---------------------------------------------------------------*/

#define SET_ACC 400     // Set Motor Acceleration
#define SET_DEC 400     // Set Motor Deceleration
#define SET_MAX 200     // Set Motor MaxSpeed
#define SET_MIN 100     // Set Motor MinSpeed
#define STOP 1000       // Set Motor Stop Position
#define TOLLERANCE 80   // Tollerance between Left and Right before Start Movement
#define RANGE_1 250     // Range 1 for Motor Speed
#define RANGE_2 500     // Range 2 for Motor Speed
#define TIMEOUT_STA 0.5 // Timeout for Ticker Status (in second)
#define TIMEOUT_DIF 0.1 // Timeout for Ticker Difference (in second)
#define TIMEOUT_POS 0.1 // Timeout for Ticker Position (in second)
#define TIMEOUT_SUN 0.5 // Timeout for Ticker SunPanel (in second)

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

int8_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
int8_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
int8_t arrest=0;       // Arrest: 0 = No, 1 = Yes
int16_t babybear=0;    // Difference (in Lux) between Left and Right
int32_t acc_data[3];   // Difference of Accelerometer
int16_t diff=0;        // Babybear or Accelerometer difference [--> Send BLE]
int16_t diff_abs=0;    // Abs of Babybear or Accelerometer difference
int8_t left=0;         // Left Command for Rotate Direction
int8_t right=0;        // Right Command for Rotate Direction
int16_t pos=0;         // Motor Position [--> Send BLE]
int16_t measure=0;     // ADC Value from SunPanel [--> Send BLE]
char DisplayStr[5];    // Display String
int16_t Status=0;      // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Send BLE]
int16_t status_bb, status_t, status_b, status_l, status_r; // Babybear Status
uint16_t TimeStamp=0;  // TimeStamp for BLE
bool tickerSta=false;
bool tickerDif=false;
bool tickerPos=false;
bool tickerSun=false;
bool BLEConnected=false;// BLE Connection Status

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

// Initializing SPI bus
DevSPI dev_spi(D11, D12, D13);

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

#ifdef FlightSense
// Initializing FlightSense Component 6180XA1
static X_NUCLEO_6180XA1 *board;
MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
#endif

#ifdef EasySpin
// Initializing EasySpin Component IHM01A1
static L6474 *motor;
#endif

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

InterruptIn mybutton(USER_BUTTON);

AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS --> Changed in Morpho PC_3

/* Update_Status ------------------------------------------------------------*/

void Update_Status(int16_t Stat)
{

    char State[12]; // Characters into State must be <12 and never =>12 or it crash!!!
    if (Stat==1) strcpy(State,"FlightSense");
    if (Stat==2) strcpy(State,"ADCSunPanel");
    if (Stat==3) strcpy(State,"Sensors");
    printf("\r\n\r\nPUSH Status: %s", State);

}

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

void User_Button_Pressed(void)
{

    Status++;

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

    Update_Status(Status);

}

#ifdef Ble

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

static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
{
    if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->enNotify(handle); 
    
    if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->enNotify(handle); 
    if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->enNotify(handle);  
    if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->enNotify(handle);
    if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->enNotify(handle);     
}

static void onUpdatesDisabledCallback(Gap::Handle_t handle)
{
    if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->disNotify(handle); 
    
    if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->disNotify(handle); 
    if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->disNotify(handle);  
    if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->disNotify(handle);
    if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->disNotify(handle);    
}

static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
{

    // if receive a manual Read request

    if (p_customsensorservice->isStatusHandle(eventDataP->handle))
    {   
        #ifdef Debug
            printf("\n\r\n\rSend BLE Display Status %d", Status);
        #endif
        p_customsensorservice->sendEnvStatus(Status, TimeStamp);
        
    } else if (p_customsensorservice->isDifferenceHandle(eventDataP->handle))
    {
        #ifdef Debug
            printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
        #endif
        p_customsensorservice->sendEnvDifference(diff, TimeStamp);

    } else if (p_customsensorservice->isPositionHandle(eventDataP->handle))
    {
        #ifdef Debug
            printf("\n\rSend BLE Position %d", pos);
        #endif    
        p_customsensorservice->sendEnvPosition(pos, TimeStamp);

    } else if (p_customsensorservice->isSunpanelHandle(eventDataP->handle))
    {
        #ifdef Debug
            printf("\n\rSend BLE Sunpanel %d mV", measure);
        #endif    
        p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);

    }
 
}

// This Callback happen when it RECEIVE a WRITE
static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
{   

    if (p_customcontrolservice->isCommandHandle(eventDataP->handle))
    {       
        // From CLIENT it receives a command in this format: Feature (4) + Type (1) + Data (2)        
        uint32_t Feature = eventDataP->data[0]<<24 | eventDataP->data[1]<<16 | eventDataP->data[2]<<8 | eventDataP->data[3];
        //uint8_t Type = eventDataP->data[4];
        int16_t Data = eventDataP->data[5]<<8 | eventDataP->data[6];
        
        //printf("\r\n\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\n\r\nReceived STATUS %d", Data);
                Status = Data;
                Update_Status(Data);
                break;
     
            case FeatureDifference:
                #ifdef Debug
                    printf("\r\n\r\nReceived DIFF %d", Data);
                #endif
                diff = Data;
                break;
 
            case FeaturePosition:
                break;
            
            case FeatureSunPanel:
                break;
                
            default:
                break;
        }
    }
    
}

static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
{
    printf("\r\n\r\nonConnectionCallback (Line %d)", __LINE__);   
    BLEConnected=true;
}

static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
{
    printf("\r\n\r\nonDisconnectionCallback (Line %d)", __LINE__);
    p_BLEdev->gap().startAdvertising();
    BLEConnected=false;
}

#endif

/* Bluetooth Initialization ---------------------------------------------------*/

bool BLE_Initialization(void)
{

#ifdef Ble

    p_BLEdev = new BLE;
    if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
    const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};        
    p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);
    
    p_BLEdev->init();

    // Set BLE CallBack Functions
    p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
    p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
    p_BLEdev->gattServer().onDataRead(onDataReadCallback);
    p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
    p_BLEdev->gap().onConnection(onConnectionCallback);
    p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
    //p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
    //p_BLEdev->gattServer().onDataSent(onDataSentCallback);    
    //p_BLEdev->gap().onTimeout(onTimeoutCallback);

    // BLE Services
    p_customcontrolservice = new CustomControlService(*p_BLEdev);   
    p_customsensorservice = new CustomSensorService(*p_BLEdev);

    // Setup BLE Advertising
    const static char DEVICE_NAME[] = BLE_DEV_NAME;
    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    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);
    p_BLEdev->gap().startAdvertising();

    printf("Initialization Ble OK (Line %d)\r\n", __LINE__);  

#endif

    return true;
    
}

/* Initialization ------------------------------------------------------------*/

bool Initialization(void)
{
    
    dev_i2c.frequency(100000);      // Default 100KHz. At 10KHz, 20KHz, 200KHz and 400KHz doesn't work
    
//----    

#ifdef FlightSense

    // Initializing FlightSense Component
    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC); // NC as Interrupt fixed from FlightSense library 42

    status_bb=board->InitBoard();
    if(status_bb) VL6180x_ErrLog("Failed to init the board!\n\r");

    // Put GPIO not used as Interrupt in Hi-Z
    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); Not Present
    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
    
    // Set Babybears
    status_l=board->sensor_left->AlsSetAnalogueGain(3);
    status_r=board->sensor_right->AlsSetAnalogueGain(3);
    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);

    printf("Init FlightSense OK\r\n");

#endif
    
//----

#ifdef EasySpin

    // Initializing EasySpin Component
    motor = new L6474(D2, D8, PB_2, D9, D10, dev_spi); // D7 conflict with BNRG_RST (when motor change direction 0-1 reset BLE)
                                                       // Used Morpho PB_2 and made HW wiring
    if (motor->Init(NULL) != COMPONENT_OK)
        return false;

    motor->SetStepMode(StepperMotor::STEP_MODE_1_8);  // Default is STEP_MODE_1_16

    // Set defaults Motor Speed
    motor->SetAcceleration(SET_ACC);
    motor->SetDeceleration(SET_DEC);
    motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
    motor->SetMinSpeed(SET_MIN);

    printf("Init EasySpin OK\r\n");

#endif

//----  

#ifdef Sensors

    // Initializing Sensors Component
    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
    accelerometer = mems->GetAccelerometer();
   
    printf("Init Sensors OK\r\n");

#endif
    
    printf("Initialization OK (Line %d)\r\n", __LINE__);    
    return true;

}

/* Measure_Babybear ----------------------------------------------------------*/

void Measure_Babybear(void)
{

#ifdef FlightSense

    status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
    status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);

    babybear = data_sensor_right.lux - data_sensor_left.lux;

    diff = babybear;

#endif

}

/* Measure_Accelerometer -----------------------------------------------------*/

void Measure_Accelerometer(void)
{

#ifdef Sensors

    accelerometer->Get_X_Axes(acc_data);

    diff = acc_data[0];

#endif

}

/* Control_Motor -------------------------------------------------------------*/

void Control_Motor(void)
{

#ifdef EasySpin

    diff_abs = abs(diff);

    //printf("Diff: %d lux/mems\n\r", diff_abs);
    motor->SetMaxSpeed(diff_abs);

    if (diff>0) { left=0; right=1; }
    if (diff<0) { left=1; right=0; }

    if (diff_abs>TOLLERANCE) {
        if (diff_abs <=RANGE_1)
        {
            if (left)  { strcpy(DisplayStr,"E___"); }
            if (right) { strcpy(DisplayStr,"___3"); }
        }
        else if (diff_abs >RANGE_1 & diff_abs <=RANGE_2)
        {
            if (left)  { strcpy(DisplayStr,"E==="); }
            if (right) { strcpy(DisplayStr,"===3"); }
        }
        else if (diff_abs >RANGE_2)
        {
            if (left)  { strcpy(DisplayStr,"E~~~"); }
            if (right) { strcpy(DisplayStr,"~~~3"); }
        }

        // In Case of Change Direction
        if (left & dir==2)  { changedir=1; }
        if (right & dir==1) { changedir=1; }

        // Run only if Stop or Change Direction
        if (diff_abs>TOLLERANCE & (dir==0 | changedir==1)) {
            if (left)   { motor->Run(StepperMotor::FWD); dir=1; changedir=0; }
            if (right)  { motor->Run(StepperMotor::BWD); dir=2; changedir=0; }
            printf("\n\rRUN");
        }
    }

    // Get Motor Position and Control Rotation Block
    pos = motor->GetPosition();
    if (pos>STOP | pos<-STOP) {
        if (pos>0) { motor->GoTo(STOP); }
        if (pos<0) { motor->GoTo(-STOP); }
        printf("\n\rGOTO");
    }

    // Stop Motor
    if (diff_abs<=TOLLERANCE) { 
        arrest=1; 
        if (Status==1) { strcpy(DisplayStr,"----"); }
        if (Status==3) { strcpy(DisplayStr,"E  3"); }
    }
        
    if (arrest==1 && dir!=0 ) { // Stop just one time
        motor->HardStop();
        dir=0;
        changedir=0;
        arrest=0;
        printf("\n\rSTOP");
    }

#endif

}

/* Measure_SolarPanel --------------------------------------------------------*/

void Measure_SolarPanel(void)
{
    
    // AnalogIn: 0V return 0.0 , 3.3V return 1.0
    measure = analog_read.read() * 3300;
    //printf("Measure = %.0f mV\r\n", measure);
    //board->display->DisplayDigit("A", 0);

    //if (abs(measure-prev_meas)>(prev_meas*0.1)) { prev_meas = measure; }
    //if (Status==2) { sprintf(DisplayStr, "%d", prev_meas); }
    
    if (Status==2) { sprintf(DisplayStr, "%d", measure); }
}

/* Ticker --------------------------------------------------------------------*/

void tickerSta_Callback(void) { tickerSta=true; }
void tickerDif_Callback(void) { tickerDif=true; }
void tickerPos_Callback(void) { tickerPos=true; }
void tickerSun_Callback(void) { tickerSun=true; }

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

int main()
{

    // Printing to the console
    printf("\r\n\r\nSunTracker by Fabio Brembilla\r\n\r\n");
       
    Initialization();
    BLE_Initialization();
    
    mybutton.fall(&User_Button_Pressed);

    #ifdef Ble
    Ticker tickerSta_ticker, tickerDif_ticker, tickerPos_ticker, tickerSun_ticker;
    tickerSta_ticker.attach(tickerSta_Callback, TIMEOUT_STA);
    #endif 
    
    // Loop until push User Button to Set 0 Point
    printf("\r\nWait PUSH Button");
    strcpy(DisplayStr,"pusH");
    while(Status==0)
    {
        #ifdef FlightSense
        board->display->DisplayString(DisplayStr, strlen(DisplayStr)); // if I set fix len=4, it aligns digit from left
        //printf("%s\n\r", DisplayStr);
        #endif
        
        #ifdef Ble
        if (p_customsensorservice->isStatusNotificationEn()) {
            if (tickerSta) {
                tickerSta=false;
                #ifdef Debug
                    printf("\n\r\n\rSend BLE Display Status %d", Status);
                #endif
                //p_customsensorservice->sendEnvStatus(Status, TimeStamp);
                p_customsensorservice->updateEnvStatus(Status, TimeStamp);    
            }
        }
        p_BLEdev->waitForEvent(); // Without it the CLIENT can't start to communicate
        #endif 
    }

    printf("\r\n\r\nStart Main Loop");

    #ifdef Ble
    tickerDif_ticker.attach(tickerDif_Callback, TIMEOUT_DIF);
    tickerPos_ticker.attach(tickerPos_Callback, TIMEOUT_POS);
    tickerSun_ticker.attach(tickerSun_Callback, TIMEOUT_SUN);
    #endif 
    
    #ifdef EasySpin
    motor->Enable(); // To put the motor on hold by execute CmdEnable
    #endif

    // Main Loop
    while(true)
    {  
        if (Status==1 | Status==2)    { Measure_Babybear(); }
        
        // If BLE is not connected, it uses Accelerometer from SERVER, otherwise receive the value from CLIENT
        if (BLEConnected==false) {
            if (Status==3)            { Measure_Accelerometer(); }
        }
        
        Control_Motor();
        Measure_SolarPanel();
        
        #ifdef FlightSense
        board->display->DisplayString(DisplayStr, strlen(DisplayStr)); // if I set fix len=4, it aligns digit from left
        //printf("%s\n\r", DisplayStr);
        #endif
        
        #ifdef Ble 
        //if (Status==3 & BLEConnected==false) { diff=0; }
        //if Status 3 and BLE not connected set diff=0 (no more useful if use SERVER Accelerometer)
        //After disconnect CLIENT, until it receive the timeout/notify to be really disconnected, it not receive diff!
        
        if (p_customsensorservice->isStatusNotificationEn()) {
            if (tickerSta) {
                tickerSta=false;
                #ifdef Debug
                    printf("\n\r\n\rSend BLE Display Status %d", Status);
                #endif
                //p_customsensorservice->sendEnvStatus(Status, TimeStamp);
                p_customsensorservice->updateEnvStatus(Status, TimeStamp);    
            }
        }
        if (p_customsensorservice->isDifferenceNotificationEn()) {
            if (tickerDif) {
                tickerDif=false;
                #ifdef Debug
                    printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
                #endif
                //p_customsensorservice->sendEnvDifference(diff, TimeStamp);
                p_customsensorservice->updateEnvDifference(diff, TimeStamp);
            }
        }
        if (p_customsensorservice->isPositionNotificationEn()) {
            if (tickerPos) {
                tickerPos=false;
                #ifdef Debug      
                    printf("\n\rSend BLE Position %d", pos);
                #endif
                //p_customsensorservice->sendEnvPosition(pos, TimeStamp);
                p_customsensorservice->updateEnvPosition(pos, TimeStamp);
            }
        }
        if (p_customsensorservice->isSunpanelNotificationEn()) {
            if (tickerSun) {
                tickerSun=false;
                #ifdef Debug   
                    printf("\n\rSend BLE SunPanel %d mV", measure); 
                #endif           
                //p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
                p_customsensorservice->updateEnvSunpanel(measure, TimeStamp);
            }
        }
         
        p_BLEdev->waitForEvent();
        #endif
    }

    //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
    //status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
}