main.cpp

00001 /*
00002 
00003 Copyright (c) 2016 Giovanni Lenzi
00004 
00005 */
00006 
00007 /*
00008  *    This firmware is a port the RedBear BLE Shield Arduino Sketch(https://github.com/RedBearLab/nRF8001/blob/master/examples/BLEControllerSketch/BLEControllerSketch.ino), 
00009  *    to Redbear Nano (http://redbearlab.com/blenano/).
00010  *    After connection of Nano to PC using the provided MK20 USB board, 
00011  *    you need to download the compiled firmware to your local disk 
00012  *    and drag and drop it to the newly created MBED drive. 
00013  *    Once flashed, you may access your Nano device with the NanoChat test application 
00014  *    or from any SAndroidE powered application (http://es3.unibs.it/SAndroidE/)
00015  */
00016 
00017 #include "mbed.h"
00018 #include "ble/BLE.h"
00019 //#include "UARTService.h"
00020 #include "Queue.h"
00021 
00022 
00023 //#define MAX_REPLY_LEN           (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN)
00024 
00025 #define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
00026 #define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
00027 #define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */
00028 
00029 #define TXRX_BUF_LEN                     20
00030 
00031 // pin modes
00032 #define PIN_INPUT                 0 // digital input pin
00033 #define PIN_OUTPUT                1 // digital output pin
00034 #define PIN_ANALOG                2 // analog pin in analogInput mode
00035 #define PIN_PWM                   3 // digital pin in PWM output mode
00036 #define PIN_SERVO                 4 // digital pin in Servo output mode
00037 #define PIN_NOTSET                5 // pin not set
00038 
00039 #define NO_GROUP                  0 // no_group means that current pin is sampled and transmitted individually
00040 
00041 
00042 #define ANALOG_MAX_VALUE          1024 // this is uint16 max value: 65535
00043 #define DEFAULT_SAMPLING_INTERVAL 1000   // 1 second
00044 #define DEFAULT_DELTA             10    // this is uint16 in range [0-65535], 655 is 1% delta
00045 
00046 BLE  ble;
00047 Queue *recvQueue = NULL, *toSendQueue =NULL;
00048 
00049 // The Nordic UART Service
00050 static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00051 static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00052 static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00053 static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
00054 
00055 uint8_t payloadTicker[TXRX_BUF_LEN] = {0,};
00056 uint8_t txPayload[TXRX_BUF_LEN] = {0,};
00057 uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
00058 
00059 GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00060 GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00061 GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
00062 GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
00063 //UARTService *m_uart_service_ptr;
00064 
00065 
00066 static const int maxPins = 30;
00067 uint8_t pinTypes[maxPins];
00068 uint8_t pinGroups[maxPins];
00069 uint16_t prevValues[maxPins];
00070 
00071 int pinSamplingIntervals[maxPins];
00072 int pinTimers[maxPins];
00073 uint16_t pinDelta[maxPins];
00074 
00075 DigitalInOut digitals[] = {P0_0,P0_7,P0_8,P0_9,P0_10,P0_11,P0_15,P0_19,P0_28,P0_29};
00076 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};
00077 AnalogIn analogs[] = {P0_1, P0_2, P0_3, P0_4, P0_5, P0_6};
00078 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};
00079 
00080 void enqueueItem(Queue *queue, uint8_t *buf, int len) {
00081     if (ble.getGapState().connected) {
00082         NODE *item = NULL;
00083         item = (NODE*) malloc(sizeof (NODE));
00084         memcpy(item->data.payload, buf, len); 
00085         item->data.length = len;
00086         Enqueue(queue, item);
00087     }
00088 }
00089 
00090 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00091 {
00092     //pc.printf("Disconnected \r\n");
00093     //pc.printf("Restart advertising \r\n");
00094     ble.startAdvertising();
00095 }
00096 
00097 
00098 NODE *enqItem = NULL;
00099 int bytesCmd = 0;
00100 bool endsOnNewLine = true;
00101 #define NOT_FOUND -1
00102 
00103 void WrittenHandler(const GattWriteCallbackParams *params)
00104 {   
00105     uint8_t buf[TXRX_BUF_LEN];
00106     uint16_t bytesRead, i;
00107 
00108     if (params->handle == txCharacteristic.getValueAttribute().getHandle()) { 
00109         ble.readCharacteristicValue(params->handle, buf, &bytesRead);
00110         
00111         if (enqItem == NULL){
00112             endsOnNewLine = buf[0]=='{';
00113         }
00114 
00115         if (!endsOnNewLine) {
00116             enqueueItem(recvQueue, buf, bytesRead);
00117         } else {
00118             for (i=0; i<bytesRead; i++) {
00119                 if (endsOnNewLine && (buf[i]=='\n' || bytesCmd>QUEUE_STRING_LENGTH)) {
00120                     if (enqItem != NULL && enqItem->data.length>0) {  
00121                         // push string to queue
00122                         Enqueue(recvQueue, enqItem);
00123                     }
00124                     enqItem = NULL;                
00125                 } else {
00126                     // enqueue character
00127                     if (enqItem == NULL) {
00128                         enqItem = (NODE*) malloc(sizeof (NODE));
00129                         bytesCmd = 0;
00130                     }
00131                     enqItem->data.payload[bytesCmd++]=buf[i];
00132                     enqItem->data.length = bytesCmd;
00133                 }
00134             }
00135         }
00136     }   
00137 }
00138 
00139 uint16_t readPin(uint8_t pin) {
00140     uint8_t mode = pinTypes[pin];
00141     if (mode==PIN_INPUT) { // exists and is initialized as digital output
00142         return digitals[mapDigitals[pin]].read()==0?0:ANALOG_MAX_VALUE;
00143     } else if (mode==PIN_OUTPUT) { // exists and is initialized as digital output
00144         return digitals[mapDigitals[pin]].read()==0?0:ANALOG_MAX_VALUE;
00145     } else if (mode==PIN_ANALOG) { // exists and is initialized as digital output
00146         return analogs[mapAnalogs[pin]].read_u16();  // 10 bits only
00147     }
00148     return 0;
00149 }
00150 
00151 uint16_t readPinOld(int pin) {
00152     uint8_t mode = pinTypes[pin];
00153     if (mode==PIN_INPUT) { // exists and is initialized as digital output
00154         mode = 0;
00155         return (((uint16_t)mode)<<8 | (uint16_t)(digitals[mapDigitals[pin]].read()));
00156     } else if (mode==PIN_OUTPUT) { // exists and is initialized as digital output
00157         mode = 1;
00158         return (((uint16_t)mode)<<8 | (uint16_t)(digitals[mapDigitals[pin]].read()));
00159     } else if (mode==PIN_ANALOG) { // exists and is initialized as digital output
00160         mode = 2;
00161         uint16_t value = analogs[mapAnalogs[pin]].read_u16();
00162         uint8_t value_lo = value;
00163         uint8_t value_hi = value>>8;
00164         mode = (value_hi << 4) | mode;
00165         return (((uint16_t)mode)<<8) | (uint16_t)value_lo;
00166     }
00167     return 0;
00168 }
00169 
00170 void sendPinValue(uint8_t pin, uint16_t value) {
00171     uint8_t buf[TXRX_BUF_LEN];
00172     buf[0]='G';
00173     buf[1]=pin;
00174     buf[2]=(uint8_t)(value>>8); 
00175     buf[3]=  (uint8_t) ((value<<8)>>8);
00176     enqueueItem(toSendQueue, buf, 4);
00177     /*int len = sprintf((char *)buf,"{\"pin\":%d,\"v\":%4.3f}",pin,(float)value/(float)ANALOG_MAX_VALUE);
00178     enqueueItem(toSendQueue,buf, len);*/
00179     
00180     prevValues[pin] = value;
00181 }
00182 
00183 
00184 void sendGroup(uint8_t groupno) {
00185     uint8_t buf[TXRX_BUF_LEN], i=0;
00186     buf[i++]='G';
00187     for (uint8_t pin=0; pin<maxPins;pin++){
00188         if (pinGroups[pin]==groupno) {
00189             uint16_t value = readPin(pin);
00190             buf[i++] = pin;
00191             buf[i++] = (uint8_t)(value>>8); 
00192             buf[i++] = (uint8_t) ((value<<8)>>8);
00193             prevValues[pin] = value;
00194         }
00195     }
00196     if (i>1) { // at least 1 pin value to send
00197         enqueueItem(toSendQueue, buf, i);
00198     }
00199 }
00200 
00201 bool isInputPin(uint8_t pin) {
00202     if (pin<maxPins){
00203         uint8_t type = pinTypes[pin];
00204         return type==PIN_INPUT||type==PIN_ANALOG;
00205     }
00206     return false;
00207 }
00208 
00209 void m_status_check_handle(void)
00210 {   
00211     for (int pin=0; pin<maxPins;pin++){
00212         if (pinTypes[pin]==PIN_INPUT||pinTypes[pin]==PIN_ANALOG) {
00213             uint16_t value = readPin(pin);
00214             //uint16_t prevValue = 33;
00215             if (prevValues[pin] != value) {
00216                 sendPinValue(pin,value);
00217             }
00218         }
00219     }
00220 }
00221 
00222 
00223 static volatile int gcd=-1;
00224 Ticker *pTicker;
00225 //Timeout timeout;
00226 static volatile bool recalcTimer = false;
00227 static volatile bool triggerSensorPolling = false;
00228 static volatile bool gcdChanged =false;
00229 
00230 int calc_gcd(int n1,int n2) {
00231     int lgcd=1;
00232     for(int i=2; i <= n1 && i <= n2; ++i)
00233     {
00234         // Checks if i is factor of both integers
00235         if(n1%i==0 && n2%i==0)
00236             lgcd = i;
00237     }
00238     return lgcd;
00239 }
00240 
00241 void check_pin_changed(void) 
00242 {
00243     uint8_t buf[QUEUE_STRING_LENGTH];
00244     if (gcd>0) {
00245         for (int pin=0; pin<maxPins;pin++){
00246             if (isInputPin(pin)) {
00247                 if (pinTimers[pin] < 0) {
00248                     pinTimers[pin] = pinSamplingIntervals[pin];
00249                 } else {
00250                     pinTimers[pin]-=gcd;
00251                 }
00252                 if (pinTimers[pin]==0) {
00253                     pinTimers[pin] = pinSamplingIntervals[pin];
00254                     uint16_t value = readPin(pin);
00255                     if (abs(prevValues[pin]-value) >= pinDelta[pin]) {
00256                         if (pinGroups[pin]!=NO_GROUP) { // enqueue sending operation for group
00257                             int len = sprintf((char *)buf,"R%c",pinGroups[pin]);
00258                             enqueueItem(recvQueue, buf, len);                        
00259                         } else { // send the pin
00260                             sendPinValue(pin,value);    
00261                         }
00262                     }
00263                 }
00264             }
00265         }
00266     }  
00267 }
00268 
00269 void calc_timer_interval()
00270 {
00271     gcd = -1;
00272     for (int pin=0; pin<maxPins;pin++){
00273         if (isInputPin(pin) && pinSamplingIntervals[pin]>0) {
00274             uint8_t buf[TXRX_BUF_LEN];
00275             int len = sprintf((char *)buf,"TIMER %d@%d",pin,pinSamplingIntervals[pin]);
00276             //int len = sprintf((char *)buf,"check-gcd");
00277             enqueueItem(toSendQueue, buf, len);
00278             
00279             if (gcd==-1) {
00280                 gcd = pinSamplingIntervals[pin];
00281             } else {
00282                 gcd = calc_gcd(gcd,pinSamplingIntervals[pin]);
00283             }
00284         }
00285     }
00286 }
00287 
00288 bool initPin(uint8_t pin, uint8_t type){
00289     bool ret=false,wasset=true,armTimer=false;
00290    
00291     //uint8_t buf[TXRX_BUF_LEN];
00292     //buf[0]='Y';buf[1]=pin;buf[2]=type;
00293     //int len = sprintf((char *)buf,"ASD%d-%c",pin,pin);
00294     //enqueueItem(toSendQueue, buf, 3);    
00295     
00296     if (pin<maxPins) {       // "initPin(): Pin number out of bounds"
00297         wasset = pinTypes[pin]!=PIN_NOTSET;
00298         if ((type==PIN_INPUT||type==PIN_OUTPUT) && mapDigitals[pin]>=0) {
00299             if (type==PIN_INPUT) digitals[mapDigitals[pin]].input();  // initialize as input
00300             if (type==PIN_OUTPUT) digitals[mapDigitals[pin]].output(); // initialize as input
00301             pinTypes[pin] = type; // mark the pin as initialized
00302             ret =true;
00303         } else if (type==PIN_ANALOG && mapAnalogs[pin]>=0) {
00304             pinTypes[pin] = type; // mark the pin as initialized
00305             ret =true;
00306         }
00307         if (!wasset && ret && (type==PIN_INPUT||type==PIN_ANALOG)) armTimer=true;
00308     }
00309     if (armTimer) {
00310         pinSamplingIntervals[pin] = DEFAULT_SAMPLING_INTERVAL;
00311         //pinTimers[pin]=pinSamplingIntervals[pin];
00312         recalcTimer = true;
00313     }
00314     
00315     return ret;
00316 }
00317 
00318 bool initPin(uint8_t pin, uint8_t type, uint8_t group){
00319     bool ret = initPin(pin, type);
00320     if (ret){
00321         pinGroups[pin]=group;
00322     }
00323     return ret;
00324 }
00325 
00326 void changeDelta(uint8_t pin, uint16_t delta) {
00327     uint8_t buf[TXRX_BUF_LEN];
00328     int len = sprintf((char *)buf,"DELTA %d@%d",pin,delta);
00329     enqueueItem(toSendQueue, buf, len);
00330     
00331     //float fdelta = delta / ANALOG_MAX_VALUE;
00332     if (delta > ANALOG_MAX_VALUE) delta=ANALOG_MAX_VALUE;
00333     if (isInputPin(pin)) {
00334         pinDelta[pin] = delta;
00335     }
00336 }
00337 
00338 void changeDeltaPercent(uint8_t pin, float fdelta) {
00339     changeDelta(pin, (uint16_t)(fdelta*ANALOG_MAX_VALUE));
00340 }
00341 void changeSamplingInterval(uint8_t pin, int interval) {
00342     if (isInputPin(pin)) {
00343         pinSamplingIntervals[pin]= interval;
00344         recalcTimer = true;
00345     }
00346 }
00347 
00348 bool writeDigital(uint8_t pin, bool value){
00349     if (mapDigitals[pin]>=0) {
00350         digitals[mapDigitals[pin]].write(value?1:0);
00351         //sendPinValue(pin,readPin(pin));
00352     }
00353 }
00354 
00355 void parseRedBearCmd(uint8_t* cmdString){
00356     uint8_t buf[TXRX_BUF_LEN];
00357     memset(buf, 0, TXRX_BUF_LEN);
00358     int len=0, scanned=-1, sampling=-1;
00359     float fdelta=-1; 
00360     
00361     uint8_t startOffset = cmdString[0]==0?1:0;
00362     uint8_t index = startOffset;
00363     uint8_t cmd = cmdString[index++], pin=cmdString[index++], mode=PIN_NOTSET, group=NO_GROUP;
00364     pin = pin>=48?pin-48:pin;
00365 
00366     switch (cmd) {
00367         case '{':
00368             //snprintf((char*) buf, MAX_REPLY_LEN, "ERROR: Unknown char\n");
00369             //m_uart_service_ptr->writeString((char*)buf);
00370             break;
00371         case 'Y':
00372             uint8_t value2write = cmdString[index++]-48;
00373             value2write = value2write>=48?value2write-48:value2write;
00374             writeDigital(pin,value2write!=0);
00375             break;
00376 
00377         case 'M': //pc.printf("Querying pin %u mode\n",pin);
00378             buf[0]=cmd;buf[1]=pin;buf[2]=pinTypes[pin];
00379             enqueueItem(toSendQueue, buf, 3);
00380             break;
00381             
00382         case 'S': // set pin mode
00383             mode = cmdString[index++];
00384             mode = mode>=48?mode-48:mode;
00385             group = cmdString[index++];
00386             if (initPin(pin, mode, group)) { // analogs are already initialized
00387             //if (initPin(pin, mode)) { // analogs are already initialized
00388                 sendPinValue(pin,readPin(pin));
00389             }
00390             break;
00391             
00392         case 'D': // delta to consider value changed (as percentage [0-1] of Voltage range)
00393             scanned = sscanf( (char *)&cmdString[2], "%f", &fdelta);
00394             
00395             if (scanned==1 && fdelta>=0 && fdelta<=1) {
00396                 len = sprintf((char *)buf,"DELTA%d@%f",(int)pin,fdelta);
00397                 enqueueItem(toSendQueue, buf, len);
00398                 changeDeltaPercent(pin, fdelta);
00399                 /*changeDelta           ( pin,((uint16_t)cmdString[index+0]) << 8  |
00400                                             ((uint16_t)cmdString[index+1]) );*/
00401             } else {
00402                 len = sprintf((char *)buf,"DELTA%d@ERR",(int)pin);
00403                 enqueueItem(toSendQueue, buf, len);
00404             }
00405             break;
00406             
00407         case 'I': // sampling interval
00408             scanned = sscanf( (char *)&cmdString[2], "%d", &sampling);
00409 
00410             if (scanned==1 && sampling>=0) {
00411                 len = sprintf((char *)buf,"SAMPL%d@%d",(int)pin,sampling);
00412                 enqueueItem(toSendQueue, buf, len);
00413                 changeSamplingInterval( pin, sampling);
00414             /*changeSamplingInterval( pin,((int)cmdString[index+0]) << 24 | 
00415                                         ((int)cmdString[index+1]) << 16 | 
00416                                         ((int)cmdString[index+2]) << 8  |
00417                                         ((int)cmdString[index+3]) );*/
00418             } else {
00419                 len = sprintf((char *)buf,"SAMPL%d@ERR",(int)pin);
00420                 enqueueItem(toSendQueue, buf, len);
00421             }
00422             break;
00423             
00424         case 'G': //pc.printf("Reading pin %u\n",pin);
00425             switch (pinTypes[pin]) {
00426                 case PIN_INPUT:
00427                 case PIN_ANALOG:
00428                     sendPinValue(pin,readPin(pin));
00429                     break;
00430                 case PIN_OUTPUT: // TODO: send warning pin not readable (is an output)
00431                 default:  // TODO: send warning pin not initialized
00432                     buf[0]=PIN_NOTSET;buf[1]=PIN_NOTSET;buf[2]=PIN_NOTSET;
00433                     enqueueItem(toSendQueue, buf, 3);
00434                     break; 
00435             }
00436             break; 
00437             
00438         case 'T':
00439             switch (pinTypes[pin]) {
00440                 case PIN_OUTPUT:
00441                     uint8_t value2write = cmdString[index++];
00442                     if (mapDigitals[pin]>=0) {
00443                         digitals[mapDigitals[pin]].write(value2write==0?0:1);
00444                         sendPinValue(pin,readPin(pin));
00445                     }
00446                     break;
00447                 case PIN_INPUT: // TODO: send warning pin not writable (is an input) 
00448                 case PIN_ANALOG: // TODO: send warning pin not writable (is an input) 
00449                 default:  // TODO: send warning pin not initialized
00450                     buf[0]='T';buf[1]='T';buf[2]='T';
00451                     enqueueItem(toSendQueue, buf, 3);
00452                     break; 
00453             }
00454             break; 
00455         case 'R':
00456             // pin variable contains the group, not the pin number
00457             sendGroup(pin);
00458             break;
00459         default:
00460             // echo received buffer
00461             enqueueItem(toSendQueue, &cmdString[startOffset], strlen((char *)&cmdString[startOffset]));
00462             break;
00463     }   
00464 }
00465 
00466 
00467 void triggerSensor(){
00468     triggerSensorPolling=true;
00469 }
00470 
00471 
00472 void changeGcdTiming(){
00473     uint8_t buf[TXRX_BUF_LEN];
00474     int len = sprintf((char *)buf,"check-gcd %d",gcd);
00475     enqueueItem(toSendQueue, buf, len);
00476 }
00477 
00478 
00479 int main(void)
00480 {
00481     
00482     for (int i=0;i<maxPins;i++) {
00483         pinTypes[i] = PIN_NOTSET;
00484         prevValues[i] = 0;
00485         pinSamplingIntervals[i] = -1;
00486         pinTimers[i]=-1;
00487         pinDelta[i]=DEFAULT_DELTA;
00488         pinGroups[i]=NO_GROUP;
00489     }
00490 
00491     ble.init();
00492     ble.onDisconnection(disconnectionCallback);
00493     ble.onDataWritten(WrittenHandler);  
00494     
00495    // setup advertising 
00496     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00497     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00498     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00499                                     (const uint8_t *)"MyNano", sizeof("MyNano") - 1);
00500     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00501                                     (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
00502     // 100ms; in multiples of 0.625ms. 
00503     ble.setAdvertisingInterval(160);
00504 
00505     ble.addService(uartService);
00506     
00507     ble.startAdvertising(); 
00508 
00509     //ticker.attach(m_status_check_handle, 0.2);
00510     
00511     // Create a UARTService object (GATT stuff).
00512     //UARTService myUartService(ble);
00513     //m_uart_service_ptr = &myUartService;
00514 
00515     Ticker ticker;
00516     //pTicker = &ticker;
00517     
00518     //Ticker pinTicker;
00519     //pinTicker.attach(triggerSensor, 5);
00520 
00521     Ticker gcdTicker;
00522     gcdTicker.attach(changeGcdTiming, 5);
00523     
00524     recvQueue = ConstructQueue(40);
00525     toSendQueue = ConstructQueue(40);
00526     
00527     uint8_t buf[QUEUE_STRING_LENGTH];
00528     NODE *deqItem = NULL;
00529      /*
00530     // set pin 7 as VCC and pin 28 as GND
00531     int len = sprintf((char *)buf,"S%c%c",7,1);
00532     enqueueItem(recvQueue, buf, len);
00533     len = sprintf((char *)buf,"S%c%c",28,1);
00534     enqueueItem(recvQueue, buf, len);
00535     len = sprintf((char *)buf,"Y%c%c",7,'1');
00536     enqueueItem(recvQueue, buf, len);
00537     len = sprintf((char *)buf,"Y%c%c",28,'0');
00538     enqueueItem(recvQueue, buf, len);*/
00539     
00540     while(1)
00541     {
00542         if (ble.getGapState().connected) {
00543             if (recalcTimer) {
00544                 recalcTimer =false;
00545                 calc_timer_interval();
00546                 //gcdChanged =true;
00547                 if (gcd>0) {
00548                     ticker.attach(NULL,5);
00549                     ticker.attach(triggerSensor, 0.001*gcd);
00550                 } else {
00551                     ticker.attach(NULL,5);
00552                 }
00553             } else if (!isEmpty(toSendQueue)) {
00554                 //while (!isEmpty(toSendQueue)) {
00555                     deqItem = Dequeue(toSendQueue);
00556                     //memset(buf, 0, QUEUE_STRING_LENGTH);  // useless
00557                     memcpy(buf, (uint8_t *)deqItem->data.payload, deqItem->data.length); 
00558                     ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, deqItem->data.length);
00559                     //ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, deqItem->data.length);
00560                     free(deqItem);
00561                 //}
00562             } else if (!isEmpty(recvQueue)) {
00563                 //if (!isEmpty(recvQueue)) {
00564                     deqItem = Dequeue(recvQueue);
00565                     memset(buf, 0, QUEUE_STRING_LENGTH); // maybe useless: TO CHECK its handling in parseRedBearCmd
00566                     memcpy(buf, (uint8_t *)deqItem->data.payload, deqItem->data.length); 
00567                     //ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, deqItem->data.length);
00568                     //ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, deqItem->data.length);
00569                     parseRedBearCmd(buf);
00570                     free(deqItem);
00571                 //}  
00572             //} else if (!isEmpty(toSendQueue)) {
00573             } else if (triggerSensorPolling) {
00574                 triggerSensorPolling = false;
00575                 check_pin_changed();
00576             } else {
00577                 ble.waitForEvent();    
00578             }
00579         } else {
00580             ble.waitForEvent();
00581         }
00582         
00583         
00584     }
00585 }
00586 
00587 
00588 
00589 
00590 
00591 
00592 
00593 
00594 
00595 
00596 
00597 
00598 
00599 
00600 
00601