Diff: main.cpp

Revision:

2:6c45738bba43

Parent:

1:ff6ec7837f46

Child:

3:16b7d807fa8c

--- a/main.cpp	Mon Mar 27 12:04:16 2017 +0000
+++ b/main.cpp	Mon Jul 24 09:36:09 2017 +0000
@@ -16,6 +16,11 @@
 
 #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. */
@@ -31,7 +36,15 @@
 #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};
@@ -47,17 +60,33 @@
 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) {
+    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");
@@ -65,23 +94,61 @@
     ble.startAdvertising();
 }
 
-bool initPin(int pin, uint8_t type){
-    bool ret=false;
-    if (pin>=0 && pin<31) {       // "initPin(): Pin number out of bounds"
-        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;
+
+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]=='{';
         }
-    }
-    return ret;
+
+        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(int pin) {
+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;
@@ -100,109 +167,43 @@
     return 0;
 }
 
-void sendPinValue(int pin, uint16_t value) {
+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;
-    ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 4);
+    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 parseRedBearCmd(){
-    uint8_t buf[TXRX_BUF_LEN];
-    memset(buf, 0, TXRX_BUF_LEN);
-    
-    uint8_t startOffset = txPayload[0]==0?1:0;
-    uint8_t index = startOffset;
-    uint8_t cmd = txPayload[index++], pin=txPayload[index++], mode=PIN_NOTSET;
-    pin = pin>=48?pin-48:pin;
 
-    switch (cmd) {
-        case 'Z':sendPinValue(pin,readPin(pin));
-            break;
-        case 'X':initPin(7, PIN_OUTPUT);
-            break;
-        case 'Y':uint8_t value2write = txPayload[index++]-48;
-                    if (mapDigitals[pin]>=0) {
-                        digitals[mapDigitals[pin]].write(value2write==0?0:1);
-                        sendPinValue(pin,readPin(pin));
-                    }
-            break;
-
-        case 'M': //pc.printf("Querying pin %u mode\n",pin);
-            buf[0]=cmd;
-            buf[1]=pin;
-            buf[2]=pinTypes[pin];
-            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3);
-            break;
-            
-        case 'S': // set pin mode
-            mode = txPayload[index++];
-            mode = mode>=48?mode-48:mode;
-            if (initPin(pin, mode)) { // analogs are already initialized
-                sendPinValue(pin,readPin(pin));
-            }
-            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;
-                    ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3);
-                    break; 
-            }
-            break; 
-            
-        case 'T':
-            switch (pinTypes[pin]) {
-                case PIN_OUTPUT:
-                    uint8_t value2write = txPayload[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';
-                    ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3);
-                    break; 
-            }
-            break; 
-            
-        default:
-            // echo received buffer
-            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), &txPayload[startOffset], strlen((char *)&txPayload[startOffset])); // FIXME
-            break;
-    }   
+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);
+    }
 }
 
-
-void WrittenHandler(const GattWriteCallbackParams *Handler)
-{   
-    uint8_t buf[TXRX_BUF_LEN];
-    uint16_t bytesRead;
-    
-    if (Handler->handle == txCharacteristic.getValueAttribute().getHandle()) //only if empty
-    {
-        ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
-        memset(txPayload, 0, TXRX_BUF_LEN);
-        memcpy(txPayload, buf, TXRX_BUF_LEN); 
-        
-        ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, bytesRead);
-        parseRedBearCmd();
+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)
@@ -218,15 +219,290 @@
     }
 }
 
+
+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(){
+    /*if (gcdChanged) {
+        gcdChanged =false;
+        if (gcd>0) {
+            pTicker->attach(NULL,5);
+            pTicker->attach(triggerSensor, 0.001*gcd);
+        } else {
+            pTicker->attach(NULL,5);
+        }
+    }*/
+
+    uint8_t buf[TXRX_BUF_LEN];
+    int len = sprintf((char *)buf,"check-gcd %d",gcd);
+    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(7,PIN_OUTPUT);
+    initPin(28,PIN_OUTPUT);
+    writeDigital(7,true);
+    writeDigital(28,false);
     
-    //memset(txPayload, 0, TXRX_BUF_LEN);
-
     ble.init();
     ble.onDisconnection(disconnectionCallback);
     ble.onDataWritten(WrittenHandler);  
@@ -245,12 +521,81 @@
     
     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;
-    ticker.attach(m_status_check_handle, 0.2);
+    //pTicker = &ticker;
+    
+    //Ticker pinTicker;
+    //pinTicker.attach(triggerSensor, 5);
+
+    Ticker gcdTicker;
+    gcdTicker.attach(changeGcdTiming, 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)
     {
-        ble.waitForEvent();
+        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();
+        }
+        
+        
     }
 }