File content as of revision 4:2ef9b332fa7c:

/*

Copyright (c) 2016 Giovanni Lenzi

*/

/*
 *    This firmware is a port the RedBear BLE Shield Arduino Sketch(https://github.com/RedBearLab/nRF8001/blob/master/examples/BLEControllerSketch/BLEControllerSketch.ino), 
 *    to Redbear Nano (http://redbearlab.com/blenano/).
 *    After connection of Nano to PC using the provided MK20 USB board, 
 *    you need to download the compiled firmware to your local disk 
 *    and drag and drop it to the newly created MBED drive. 
 *    Once flashed, you may access your Nano device with the NanoChat test application 
 *    or from any SAndroidE powered application (http://es3.unibs.it/SAndroidE/)
 */

#include "mbed.h"
#include "ble/BLE.h"
//#include "UARTService.h"
#include "Queue.h"


//#define MAX_REPLY_LEN           (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN)

#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */

#define TXRX_BUF_LEN                     20

// pin modes
#define PIN_INPUT                 0 // digital input pin
#define PIN_OUTPUT                1 // digital output pin
#define PIN_ANALOG                2 // analog pin in analogInput mode
#define PIN_PWM                   3 // digital pin in PWM output mode
#define PIN_SERVO                 4 // digital pin in Servo output mode
#define PIN_NOTSET                5 // pin not set

#define NO_GROUP                  0 // no_group means that current pin is sampled and transmitted individually


#define ANALOG_MAX_VALUE          1024 // this is uint16 max value: 65535
#define DEFAULT_SAMPLING_INTERVAL 1000   // 1 second
#define DEFAULT_DELTA             10    // this is uint16 in range [0-65535], 655 is 1% delta

BLE  ble;
Queue *recvQueue = NULL, *toSendQueue =NULL;

// The Nordic UART Service
static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};

uint8_t payloadTicker[TXRX_BUF_LEN] = {0,};
uint8_t txPayload[TXRX_BUF_LEN] = {0,};
uint8_t rxPayload[TXRX_BUF_LEN] = {0,};

GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
//UARTService *m_uart_service_ptr;


static const int maxPins = 30;
uint8_t pinTypes[maxPins];
uint8_t pinGroups[maxPins];
uint16_t prevValues[maxPins];

int pinSamplingIntervals[maxPins];
int pinTimers[maxPins];
uint16_t pinDelta[maxPins];

DigitalInOut digitals[] = {P0_0,P0_7,P0_8,P0_9,P0_10,P0_11,P0_15,P0_19,P0_28,P0_29};
int mapDigitals[] = { 0,-1,-1,-1,-1,-1,-1, 1,2,3,4,5,-1,-1,-1, 6,-1,-1,-1, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 9,-1};
AnalogIn analogs[] = {P0_1, P0_2, P0_3, P0_4, P0_5, P0_6};
int mapAnalogs[] =  {-1, 0, 1, 2, 3, 4, 5,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};

void enqueueItem(Queue *queue, uint8_t *buf, int len) {
    if (ble.getGapState().connected) {
        NODE *item = NULL;
        item = (NODE*) malloc(sizeof (NODE));
        memcpy(item->data.payload, buf, len); 
        item->data.length = len;
        Enqueue(queue, item);
    }
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    //pc.printf("Disconnected \r\n");
    //pc.printf("Restart advertising \r\n");
    ble.startAdvertising();
}


NODE *enqItem = NULL;
int bytesCmd = 0;
bool endsOnNewLine = true;
#define NOT_FOUND -1

void WrittenHandler(const GattWriteCallbackParams *params)
{   
    uint8_t buf[TXRX_BUF_LEN];
    uint16_t bytesRead, i;

    if (params->handle == txCharacteristic.getValueAttribute().getHandle()) { 
        ble.readCharacteristicValue(params->handle, buf, &bytesRead);
        
        if (enqItem == NULL){
            endsOnNewLine = buf[0]=='{';
        }

        if (!endsOnNewLine) {
            enqueueItem(recvQueue, buf, bytesRead);
        } else {
            for (i=0; i<bytesRead; i++) {
                if (endsOnNewLine && (buf[i]=='\n' || bytesCmd>QUEUE_STRING_LENGTH)) {
                    if (enqItem != NULL && enqItem->data.length>0) {  
                        // push string to queue
                        Enqueue(recvQueue, enqItem);
                    }
                    enqItem = NULL;                
                } else {
                    // enqueue character
                    if (enqItem == NULL) {
                        enqItem = (NODE*) malloc(sizeof (NODE));
                        bytesCmd = 0;
                    }
                    enqItem->data.payload[bytesCmd++]=buf[i];
                    enqItem->data.length = bytesCmd;
                }
            }
        }
    }   
}

uint16_t readPin(uint8_t pin) {
    uint8_t mode = pinTypes[pin];
    if (mode==PIN_INPUT) { // exists and is initialized as digital output
        return digitals[mapDigitals[pin]].read()==0?0:ANALOG_MAX_VALUE;
    } else if (mode==PIN_OUTPUT) { // exists and is initialized as digital output
        return digitals[mapDigitals[pin]].read()==0?0:ANALOG_MAX_VALUE;
    } else if (mode==PIN_ANALOG) { // exists and is initialized as digital output
        return analogs[mapAnalogs[pin]].read_u16();  // 10 bits only
    }
    return 0;
}

uint16_t readPinOld(int pin) {
    uint8_t mode = pinTypes[pin];
    if (mode==PIN_INPUT) { // exists and is initialized as digital output
        mode = 0;
        return (((uint16_t)mode)<<8 | (uint16_t)(digitals[mapDigitals[pin]].read()));
    } else if (mode==PIN_OUTPUT) { // exists and is initialized as digital output
        mode = 1;
        return (((uint16_t)mode)<<8 | (uint16_t)(digitals[mapDigitals[pin]].read()));
    } else if (mode==PIN_ANALOG) { // exists and is initialized as digital output
        mode = 2;
        uint16_t value = analogs[mapAnalogs[pin]].read_u16();
        uint8_t value_lo = value;
        uint8_t value_hi = value>>8;
        mode = (value_hi << 4) | mode;
        return (((uint16_t)mode)<<8) | (uint16_t)value_lo;
    }
    return 0;
}

void sendPinValue(uint8_t pin, uint16_t value) {
    uint8_t buf[TXRX_BUF_LEN];
    buf[0]='G';
    buf[1]=pin;
    buf[2]=(uint8_t)(value>>8); 
    buf[3]=  (uint8_t) ((value<<8)>>8);
    enqueueItem(toSendQueue, buf, 4);
    /*int len = sprintf((char *)buf,"{\"pin\":%d,\"v\":%4.3f}",pin,(float)value/(float)ANALOG_MAX_VALUE);
    enqueueItem(toSendQueue,buf, len);*/
    
    prevValues[pin] = value;
}


void sendGroup(uint8_t groupno) {
    uint8_t buf[TXRX_BUF_LEN], i=0;
    buf[i++]='G';
    for (uint8_t pin=0; pin<maxPins;pin++){
        if (pinGroups[pin]==groupno) {
            uint16_t value = readPin(pin);
            buf[i++] = pin;
            buf[i++] = (uint8_t)(value>>8); 
            buf[i++] = (uint8_t) ((value<<8)>>8);
            prevValues[pin] = value;
        }
    }
    if (i>1) { // at least 1 pin value to send
        enqueueItem(toSendQueue, buf, i);
    }
}

bool isInputPin(uint8_t pin) {
    if (pin<maxPins){
        uint8_t type = pinTypes[pin];
        return type==PIN_INPUT||type==PIN_ANALOG;
    }
    return false;
}

void m_status_check_handle(void)
{   
    for (int pin=0; pin<maxPins;pin++){
        if (pinTypes[pin]==PIN_INPUT||pinTypes[pin]==PIN_ANALOG) {
            uint16_t value = readPin(pin);
            //uint16_t prevValue = 33;
            if (prevValues[pin] != value) {
                sendPinValue(pin,value);
            }
        }
    }
}


static volatile int gcd=-1;
Ticker *pTicker;
//Timeout timeout;
static volatile bool recalcTimer = false;
static volatile bool triggerSensorPolling = false;
static volatile bool gcdChanged =false;

int calc_gcd(int n1,int n2) {
    int lgcd=1;
    for(int i=2; i <= n1 && i <= n2; ++i)
    {
        // Checks if i is factor of both integers
        if(n1%i==0 && n2%i==0)
            lgcd = i;
    }
    return lgcd;
}

void check_pin_changed(void) 
{
    uint8_t buf[QUEUE_STRING_LENGTH];
    if (gcd>0) {
        for (int pin=0; pin<maxPins;pin++){
            if (isInputPin(pin)) {
                if (pinTimers[pin] < 0) {
                    pinTimers[pin] = pinSamplingIntervals[pin];
                } else {
                    pinTimers[pin]-=gcd;
                }
                if (pinTimers[pin]==0) {
                    pinTimers[pin] = pinSamplingIntervals[pin];
                    uint16_t value = readPin(pin);
                    if (abs(prevValues[pin]-value) >= pinDelta[pin]) {
                        if (pinGroups[pin]!=NO_GROUP) { // enqueue sending operation for group
                            int len = sprintf((char *)buf,"R%c",pinGroups[pin]);
                            enqueueItem(recvQueue, buf, len);                        
                        } else { // send the pin
                            sendPinValue(pin,value);    
                        }
                    }
                }
            }
        }
    }  
}

void calc_timer_interval()
{
    gcd = -1;
    for (int pin=0; pin<maxPins;pin++){
        if (isInputPin(pin) && pinSamplingIntervals[pin]>0) {
            uint8_t buf[TXRX_BUF_LEN];
            int len = sprintf((char *)buf,"TIMER %d@%d",pin,pinSamplingIntervals[pin]);
            //int len = sprintf((char *)buf,"check-gcd");
            enqueueItem(toSendQueue, buf, len);
            
            if (gcd==-1) {
                gcd = pinSamplingIntervals[pin];
            } else {
                gcd = calc_gcd(gcd,pinSamplingIntervals[pin]);
            }
        }
    }
}

bool initPin(uint8_t pin, uint8_t type){
    bool ret=false,wasset=true,armTimer=false;
   
    //uint8_t buf[TXRX_BUF_LEN];
    //buf[0]='Y';buf[1]=pin;buf[2]=type;
    //int len = sprintf((char *)buf,"ASD%d-%c",pin,pin);
    //enqueueItem(toSendQueue, buf, 3);    
    
    if (pin<maxPins) {       // "initPin(): Pin number out of bounds"
        wasset = pinTypes[pin]!=PIN_NOTSET;
        if ((type==PIN_INPUT||type==PIN_OUTPUT) && mapDigitals[pin]>=0) {
            if (type==PIN_INPUT) digitals[mapDigitals[pin]].input();  // initialize as input
            if (type==PIN_OUTPUT) digitals[mapDigitals[pin]].output(); // initialize as input
            pinTypes[pin] = type; // mark the pin as initialized
            ret =true;
        } else if (type==PIN_ANALOG && mapAnalogs[pin]>=0) {
            pinTypes[pin] = type; // mark the pin as initialized
            ret =true;
        }
        if (!wasset && ret && (type==PIN_INPUT||type==PIN_ANALOG)) armTimer=true;
    }
    if (armTimer) {
        pinSamplingIntervals[pin] = DEFAULT_SAMPLING_INTERVAL;
        //pinTimers[pin]=pinSamplingIntervals[pin];
        recalcTimer = true;
    }
    
    return ret;
}

bool initPin(uint8_t pin, uint8_t type, uint8_t group){
    bool ret = initPin(pin, type);
    if (ret){
        pinGroups[pin]=group;
    }
    return ret;
}

void changeDelta(uint8_t pin, uint16_t delta) {
    uint8_t buf[TXRX_BUF_LEN];
    int len = sprintf((char *)buf,"DELTA %d@%d",pin,delta);
    enqueueItem(toSendQueue, buf, len);
    
    //float fdelta = delta / ANALOG_MAX_VALUE;
    if (delta > ANALOG_MAX_VALUE) delta=ANALOG_MAX_VALUE;
    if (isInputPin(pin)) {
        pinDelta[pin] = delta;
    }
}

void changeDeltaPercent(uint8_t pin, float fdelta) {
    changeDelta(pin, (uint16_t)(fdelta*ANALOG_MAX_VALUE));
}
void changeSamplingInterval(uint8_t pin, int interval) {
    if (isInputPin(pin)) {
        pinSamplingIntervals[pin]= interval;
        recalcTimer = true;
    }
}

bool writeDigital(uint8_t pin, bool value){
    if (mapDigitals[pin]>=0) {
        digitals[mapDigitals[pin]].write(value?1:0);
        //sendPinValue(pin,readPin(pin));
    }
}

void parseRedBearCmd(uint8_t* cmdString){
    uint8_t buf[TXRX_BUF_LEN];
    memset(buf, 0, TXRX_BUF_LEN);
    int len=0, scanned=-1, sampling=-1;
    float fdelta=-1; 
    
    uint8_t startOffset = cmdString[0]==0?1:0;
    uint8_t index = startOffset;
    uint8_t cmd = cmdString[index++], pin=cmdString[index++], mode=PIN_NOTSET, group=NO_GROUP;
    pin = pin>=48?pin-48:pin;

    switch (cmd) {
        case '{':
            //snprintf((char*) buf, MAX_REPLY_LEN, "ERROR: Unknown char\n");
            //m_uart_service_ptr->writeString((char*)buf);
            break;
        case 'Y':
            uint8_t value2write = cmdString[index++]-48;
            value2write = value2write>=48?value2write-48:value2write;
            writeDigital(pin,value2write!=0);
            break;

        case 'M': //pc.printf("Querying pin %u mode\n",pin);
            buf[0]=cmd;buf[1]=pin;buf[2]=pinTypes[pin];
            enqueueItem(toSendQueue, buf, 3);
            break;
            
        case 'S': // set pin mode
            mode = cmdString[index++];
            mode = mode>=48?mode-48:mode;
            group = cmdString[index++];
            if (initPin(pin, mode, group)) { // analogs are already initialized
            //if (initPin(pin, mode)) { // analogs are already initialized
                sendPinValue(pin,readPin(pin));
            }
            break;
            
        case 'D': // delta to consider value changed (as percentage [0-1] of Voltage range)
            scanned = sscanf( (char *)&cmdString[2], "%f", &fdelta);
            
            if (scanned==1 && fdelta>=0 && fdelta<=1) {
                len = sprintf((char *)buf,"DELTA%d@%f",(int)pin,fdelta);
                enqueueItem(toSendQueue, buf, len);
                changeDeltaPercent(pin, fdelta);
                /*changeDelta           ( pin,((uint16_t)cmdString[index+0]) << 8  |
                                            ((uint16_t)cmdString[index+1]) );*/
            } else {
                len = sprintf((char *)buf,"DELTA%d@ERR",(int)pin);
                enqueueItem(toSendQueue, buf, len);
            }
            break;
            
        case 'I': // sampling interval
            scanned = sscanf( (char *)&cmdString[2], "%d", &sampling);

            if (scanned==1 && sampling>=0) {
                len = sprintf((char *)buf,"SAMPL%d@%d",(int)pin,sampling);
                enqueueItem(toSendQueue, buf, len);
                changeSamplingInterval( pin, sampling);
            /*changeSamplingInterval( pin,((int)cmdString[index+0]) << 24 | 
                                        ((int)cmdString[index+1]) << 16 | 
                                        ((int)cmdString[index+2]) << 8  |
                                        ((int)cmdString[index+3]) );*/
            } else {
                len = sprintf((char *)buf,"SAMPL%d@ERR",(int)pin);
                enqueueItem(toSendQueue, buf, len);
            }
            break;
            
        case 'G': //pc.printf("Reading pin %u\n",pin);
            switch (pinTypes[pin]) {
                case PIN_INPUT:
                case PIN_ANALOG:
                    sendPinValue(pin,readPin(pin));
                    break;
                case PIN_OUTPUT: // TODO: send warning pin not readable (is an output)
                default:  // TODO: send warning pin not initialized
                    buf[0]=PIN_NOTSET;buf[1]=PIN_NOTSET;buf[2]=PIN_NOTSET;
                    enqueueItem(toSendQueue, buf, 3);
                    break; 
            }
            break; 
            
        case 'T':
            switch (pinTypes[pin]) {
                case PIN_OUTPUT:
                    uint8_t value2write = cmdString[index++];
                    if (mapDigitals[pin]>=0) {
                        digitals[mapDigitals[pin]].write(value2write==0?0:1);
                        sendPinValue(pin,readPin(pin));
                    }
                    break;
                case PIN_INPUT: // TODO: send warning pin not writable (is an input) 
                case PIN_ANALOG: // TODO: send warning pin not writable (is an input) 
                default:  // TODO: send warning pin not initialized
                    buf[0]='T';buf[1]='T';buf[2]='T';
                    enqueueItem(toSendQueue, buf, 3);
                    break; 
            }
            break; 
        case 'R':
            // pin variable contains the group, not the pin number
            sendGroup(pin);
            break;
        default:
            // echo received buffer
            enqueueItem(toSendQueue, &cmdString[startOffset], strlen((char *)&cmdString[startOffset]));
            break;
    }   
}


void triggerSensor(){
    triggerSensorPolling=true;
}


void changeGcdTiming(){
    uint8_t buf[TXRX_BUF_LEN];
    int len = sprintf((char *)buf,"check-gcd %d",gcd);
    enqueueItem(toSendQueue, buf, len);
}

AnalogIn ain(P0_6);
void fnCustomParameter() {
    unsigned short val,max;
    int i;
    if (ble.getGapState().connected) {
        i=0;
        max=0;
        while(i++<300) {  // read the max of 3 consecutive samples = 3*100 (100=10ms/100us)
            val = ain.read_u16();
            if (val>max) max=val;
            wait_us(100);
        }
        
        // enqueue into bluetooth queue
        uint8_t buf[TXRX_BUF_LEN];
        int len = sprintf((char *)buf,"W%d",val);
        enqueueItem(toSendQueue, buf, len);
    }
}


int main(void)
{
    for (int i=0;i<maxPins;i++) {
        pinTypes[i] = PIN_NOTSET;
        prevValues[i] = 0;
        pinSamplingIntervals[i] = -1;
        pinTimers[i]=-1;
        pinDelta[i]=DEFAULT_DELTA;
        pinGroups[i]=NO_GROUP;
    }

    initPin(28,PIN_OUTPUT);
    initPin(29,PIN_OUTPUT);
    writeDigital(28,0);
    writeDigital(29,1);

    ble.init();
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(WrittenHandler);  
    
   // setup advertising 
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                    (const uint8_t *)"MyNano", sizeof("MyNano") - 1);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                    (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
    // 100ms; in multiples of 0.625ms. 
    ble.setAdvertisingInterval(160);

    ble.addService(uartService);
    
    ble.startAdvertising(); 

    //ticker.attach(m_status_check_handle, 0.2);
    
    // Create a UARTService object (GATT stuff).
    //UARTService myUartService(ble);
    //m_uart_service_ptr = &myUartService;

    Ticker ticker;
    //pTicker = &ticker;
    
    //Ticker pinTicker;
    //pinTicker.attach(triggerSensor, 5);

    Ticker gcdTicker;
    gcdTicker.attach(changeGcdTiming, 5);

    Ticker customParameterTicker;
    customParameterTicker.attach(fnCustomParameter, 5);
    
    recvQueue = ConstructQueue(40);
    toSendQueue = ConstructQueue(40);
    
    uint8_t buf[QUEUE_STRING_LENGTH];
    NODE *deqItem = NULL;
     /*
    // set pin 7 as VCC and pin 28 as GND
    int len = sprintf((char *)buf,"S%c%c",7,1);
    enqueueItem(recvQueue, buf, len);
    len = sprintf((char *)buf,"S%c%c",28,1);
    enqueueItem(recvQueue, buf, len);
    len = sprintf((char *)buf,"Y%c%c",7,'1');
    enqueueItem(recvQueue, buf, len);
    len = sprintf((char *)buf,"Y%c%c",28,'0');
    enqueueItem(recvQueue, buf, len);*/
    
    while(1)
    {
        if (ble.getGapState().connected) {
            if (recalcTimer) {
                recalcTimer =false;
                calc_timer_interval();
                //gcdChanged =true;
                if (gcd>0) {
                    ticker.attach(NULL,5);
                    ticker.attach(triggerSensor, 0.001*gcd);
                } else {
                    ticker.attach(NULL,5);
                }
            } else if (!isEmpty(toSendQueue)) {
                //while (!isEmpty(toSendQueue)) {
                    deqItem = Dequeue(toSendQueue);
                    //memset(buf, 0, QUEUE_STRING_LENGTH);  // useless
                    memcpy(buf, (uint8_t *)deqItem->data.payload, deqItem->data.length); 
                    ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, deqItem->data.length);
                    //ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, deqItem->data.length);
                    free(deqItem);
                //}
            } else if (!isEmpty(recvQueue)) {
                //if (!isEmpty(recvQueue)) {
                    deqItem = Dequeue(recvQueue);
                    memset(buf, 0, QUEUE_STRING_LENGTH); // maybe useless: TO CHECK its handling in parseRedBearCmd
                    memcpy(buf, (uint8_t *)deqItem->data.payload, deqItem->data.length); 
                    //ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, deqItem->data.length);
                    //ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, deqItem->data.length);
                    parseRedBearCmd(buf);
                    free(deqItem);
                //}  
            //} else if (!isEmpty(toSendQueue)) {
            } else if (triggerSensorPolling) {
                triggerSensorPolling = false;
                check_pin_changed();
            } else {
                ble.waitForEvent();    
            }
        } else {
            ble.waitForEvent();
        }
        
        
    }
}