Diff: main.cpp

Revision:

7:eb2215fe86b2

Parent:

6:ce0e5024e3fa

Child:

8:63b01a643f4d

--- a/main.cpp	Tue Apr 12 09:23:43 2016 +0000
+++ b/main.cpp	Tue Apr 19 08:53:53 2016 +0000
@@ -3,7 +3,7 @@
  * @file    main.cpp
  * @author  Fabio Brembilla
  * @version V2.0.0
- * @date    March, 2016
+ * @date    April, 2016
  * @brief   SunTracker + BLE (Client) Vertical Application
  *          This application use IKS01A1, IDB0XA1 expansion boards
  ******************************************************************************
@@ -43,9 +43,6 @@
 /* Includes ------------------------------------------------------------------*/
 
 #include "mbed.h"
-#include "ble/BLE.h"
-#include "ble/DiscoveredCharacteristic.h"
-#include "ble/DiscoveredService.h"
 
 #ifdef Sensors
     #include "DevI2C.h"
@@ -54,52 +51,75 @@
 
 /* BlueTooth -----------------------------------------------------------------*/
 
+#include "BLE.h"
+#include "UUID.h"
+#include "DiscoveredCharacteristic.h"
+#include "DiscoveredService.h"
 #include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32
 
 #define SCAN_INT  0x30 // 30 ms = 48 * 0.625 ms
 #define SCAN_WIND 0x30 // 30 ms = 48 * 0.625 ms
 
-const Gap::Address_t  BLE_address_BE       = {0xFF, 0xEE, 0xDD, 0xCC, 0xBB, 0xAA};  // CLIENT address
-const Gap::Address_t  BLE_peer_address_BE  = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF};  // SERVER address (must be the same of SERVER)
-//const Gap::Address_t  BLE_address_BE       = {0xCC, 0x00, 0x00, 0xE1, 0x80, 0x02};  // CLIENT address
-//const Gap::Address_t  BLE_peer_address_BE  = {0xFD, 0x66, 0x05, 0x13, 0xBE, 0xBA};  // SERVER address (must be the same of SERVER)
+// 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];
+
+//const Gap::Address_t  BLE_address_BE       = {0xFF, 0xEE, 0xDD, 0xCC, 0xBB, 0xAA};  // CLIENT address
+//const Gap::Address_t  BLE_peer_address_BE  = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF};  // SERVER address (must be the same of SERVER)
+const Gap::Address_t  BLE_address_BE       = {0xCC, 0x00, 0x00, 0xE1, 0x80, 0x02};  // CLIENT address
+const Gap::Address_t  BLE_peer_address_BE  = {0xFD, 0x66, 0x05, 0x13, 0xBE, 0xBA};  // SERVER address (must be the same of SERVER)
 
-DiscoveredCharacteristic userbutton_bleCharacteristic;
+//const UUID::ShortUUIDBytes_t CONTROL_COMMAND_CHAR_UUID  = 0xA001;
+//const UUID::ShortUUIDBytes_t SENS_STATUS_CHAR_UUID      = 0xB001;
+//const UUID::ShortUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID  = 0xB002;
+//const UUID::ShortUUIDBytes_t SENS_POSITION_CHAR_UUID    = 0xB003;
+//const UUID::ShortUUIDBytes_t SENS_SUNPANEL_CHAR_UUID    = 0xB004;
+const UUID::LongUUIDBytes_t CONTROL_COMMAND_CHAR_UUID = {0x00,0x00,0x00,0x02,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+const UUID::LongUUIDBytes_t SENS_STATUS_CHAR_UUID     = {0x00,0x00,0x04,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+const UUID::LongUUIDBytes_t SENS_DIFFERENCE_CHAR_UUID = {0x00,0x00,0x08,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+const UUID::LongUUIDBytes_t SENS_POSITION_CHAR_UUID   = {0x00,0x00,0x10,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+const UUID::LongUUIDBytes_t SENS_SUNPANEL_CHAR_UUID   = {0x00,0x00,0x20,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
+
+DiscoveredCharacteristic command_bleCharacteristic;
+DiscoveredCharacteristic status_bleCharacteristic;
 DiscoveredCharacteristic difference_bleCharacteristic;
 DiscoveredCharacteristic position_bleCharacteristic;
 DiscoveredCharacteristic sunpanel_bleCharacteristic;
 
-#define SIZEOF_CONTROL_DATA_LEN     2
-#define DIFFERENCE_DATA_LEN         2
+#define COMMAND_DATA_LEN        2+4+1+2 // TimeStamp (only for Response) + Feature + Type + Data
 // 1 byte   0xFF     8 bit      int8
 // 2 byte   0xFFFF   16 bit     int16
 
-uint8_t value_write[SIZEOF_CONTROL_DATA_LEN];
-int16_t value_read = 0;
-uint8_t value_write_diff[DIFFERENCE_DATA_LEN];
+uint8_t value_write[COMMAND_DATA_LEN];
+
+#define FeatureStatus      0x00000400
+#define FeatureDifference  0x00000800
+#define FeaturePosition    0x00001000
+#define FeatureSunPanel    0x00002000
 
 /* Variables ------------------------------------------------------------------*/
 
 enum {
   IDLE = 0,
-  READ_BUT  = 1,
+  READ_STA  = 1,
   READ_DIF  = 2,
   READ_POS  = 3,
   READ_SUN  = 4,
-  WRITE_BUT = 5,
+  WRITE_STA = 5,
   WRITE_DIF = 6
 };
 int trigger_Op = IDLE;
 bool trigger_button = false;
 
-int8_t display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control [--> Receive BLE]
+int16_t Status=0;      // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Receive BLE]
 int16_t diff=0;        // Babybear or Accelerometer difference [--> Receive BLE]
 int16_t pos=0;         // Motor Position [--> Receive BLE]
 int16_t measure=0;     // ADC Value from SunPanel [--> Receive BLE]
 
 /* Initializations ------------------------------------------------------------*/
 
-#ifdef Sensors    
+#ifdef Sensors
 
 // Initializing I2C bus
 DevI2C dev_i2c(D14, D15);
@@ -110,6 +130,7 @@
 
 int32_t acc_data[3];    // Accelerometer difference
 int16_t acc_diff=0;     // Accelerometer difference
+
 #endif
 
 InterruptIn mybutton(USER_BUTTON);
@@ -119,6 +140,15 @@
 void User_Button_Pressed(void)
 {
     trigger_button = true;
+    
+    Status++;
+
+#ifdef Sensors
+    if (Status>3)  { Status=1; }
+#else
+    if (Status>2)  { Status=1; }
+#endif 
+    
 }
 
 /* Bluetooth CallBack ---------------------------------------------------------*/
@@ -146,7 +176,7 @@
 void serviceDiscoveryCallback(const DiscoveredService *service)
 {
     printf("Start Service Discovery\r\n");
-/*
+
     if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT)
     {
         printf("S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
@@ -161,33 +191,56 @@
         }
         printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
     }
-*/    
+  
 }
 
 void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
 {   
-    if (characteristicP->getUUID().getShortUUID() == 0xA001)
-    {
-        userbutton_bleCharacteristic = *characteristicP;
-        printf("Found 0xA001 Characteristic\r\n");
+    int COMMAND_CHAR_count=0, STATUS_CHAR_count=0, DIFFERENCE_CHAR_count=0, POSITION_CHAR_count=0, SUNPANEL_CHAR_count=0;
+    const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
+    
+    for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++)
+    { 
+        if (longUUIDBytes[i] == CONTROL_COMMAND_CHAR_UUID[i])   COMMAND_CHAR_count++;
+        if (longUUIDBytes[i] == SENS_STATUS_CHAR_UUID[i])       STATUS_CHAR_count++;
+        if (longUUIDBytes[i] == SENS_DIFFERENCE_CHAR_UUID[i])   DIFFERENCE_CHAR_count++;
+        if (longUUIDBytes[i] == SENS_POSITION_CHAR_UUID[i])     POSITION_CHAR_count++;
+        if (longUUIDBytes[i] == SENS_SUNPANEL_CHAR_UUID[i])     SUNPANEL_CHAR_count++;
     }
     
-    if (characteristicP->getUUID().getShortUUID() == 0xB001)
+    //if (characteristicP->getUUID().getShortUUID() == CONTROL_COMMAND_CHAR_UUID)
+    if (COMMAND_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
+    {
+        command_bleCharacteristic = *characteristicP;
+        printf("Found Command Characteristic\r\n");
+    }
+
+    //if (characteristicP->getUUID().getShortUUID() == SENS_STATUS_CHAR_UUID)
+    if (STATUS_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
+    {
+        status_bleCharacteristic = *characteristicP;    
+        printf("Found Status Characteristic\r\n");
+    }
+
+    //if (characteristicP->getUUID().getShortUUID() == SENS_DIFFERENCE_CHAR_UUID)    
+    if (DIFFERENCE_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
     {
         difference_bleCharacteristic = *characteristicP;    
-        printf("Found 0xB001 Characteristic\r\n");
+        printf("Found Difference Characteristic\r\n");
     }
-    
-    if (characteristicP->getUUID().getShortUUID() == 0xB002)
+
+    //if (characteristicP->getUUID().getShortUUID() == SENS_POSITION_CHAR_UUID)    
+    if (POSITION_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
     {
         position_bleCharacteristic = *characteristicP;
-        printf("Found 0xB002 Characteristic\r\n");
+        printf("Found Position Characteristic\r\n");
     }
-    
-    if (characteristicP->getUUID().getShortUUID() == 0xB003)
+
+    //if (characteristicP->getUUID().getShortUUID() == SENS_SUNPANEL_CHAR_UUID)    
+    if (SUNPANEL_CHAR_count == UUID::LENGTH_OF_LONG_UUID)
     {
         sunpanel_bleCharacteristic = *characteristicP;
-        printf("Found 0xB003 Characteristic\r\n");
+        printf("Found Sunpanel Characteristic\r\n");
     }
 }
 
@@ -196,38 +249,32 @@
     //printf("discoveryTerminationCallback for handle %u\r\n", connectionHandle);
     printf("Stop Service Discovery\r\n\r\n");
     
-    trigger_Op = READ_BUT;
+    trigger_Op = READ_STA;
 }
 
 void onDataReadCallback(const GattReadCallbackParams *response)
 {
     // Read SERVER --> CLIENT
     
-    /*
-    printf("response Handle %d\r\n", response->handle);
-    printf("userbutton Handle %d\r\n", userbutton_bleCharacteristic.getValueHandle());
-    printf("difference Handle %d\r\n", difference_bleCharacteristic.getValueHandle());
-    printf("position Handle %d\r\n", position_bleCharacteristic.getValueHandle());
-    printf("sunpanel Handle %d\r\n", sunpanel_bleCharacteristic.getValueHandle());
+    // From SERVER it receives a sensor in this format: TimeStamp (2) + Data (2)
+    uint16_t TimeStamp = response->data[0]<<8 | response->data[1];
+    int16_t Data = response->data[2] | response->data[3]<<8;       // Return original value after inverted with STORE_LE_16 into SERVER before send by BLE
     
-    printf("onDataRead (data[0] %x)\r\n", response->data[0]);
-    printf("onDataRead (data[1] %x)\r\n\r\n", response->data[1]);   
-    */
+    //printf("\r\n\ronDataReadCallback (TimeStamp %x)", TimeStamp);
+    //printf("\n\ronDataReadCallback (Data Hex %x)", Data);
+    //printf("\n\ronDataReadCallback (Data Dec %d)", Data);   
     
-    // Return original value after inverted with STORE_LE_16 to send by BLE
-    value_read = response->data[0] + (response->data[1]<<8);    
-    
-    if (response->handle == userbutton_bleCharacteristic.getValueHandle())
+    if (response->handle == status_bleCharacteristic.getValueHandle())
     {     
-        display = value_read;         
-        printf("\n\rReceive BLE Display %d\n\r", display);
+        Status = Data;         
+        printf("\n\rReceive BLE Display Status %d\n\r", Status);
         
         trigger_Op = READ_DIF;
     }
     
     if (response->handle == difference_bleCharacteristic.getValueHandle())
     {    
-        diff = value_read;      
+        diff = Data;      
         printf("Receive BLE Difference %d lux/mems\n\r", diff);
         
         trigger_Op = READ_POS;
@@ -235,7 +282,7 @@
     
     if (response->handle == position_bleCharacteristic.getValueHandle())
     {   
-        pos = value_read;
+        pos = Data;
         printf("Receive BLE Position %d\n\r", pos);
         
         trigger_Op = READ_SUN;
@@ -243,10 +290,10 @@
     
     if (response->handle == sunpanel_bleCharacteristic.getValueHandle())
     {   
-        measure = value_read;         
+        measure = Data;         
         printf("Receive BLE Sunpanel %d mV\n\r", measure);
         
-        trigger_Op = WRITE_BUT;
+        trigger_Op = WRITE_STA;
     }
 }
 
@@ -254,25 +301,45 @@
 {
     // Write CLIENT --> SERVER
 
-    //printf("myonDataWrite (data[0] %x)\n\r", response->data[0]);
-    //printf("myonDataWrite (data[1] %x)\n\r\n\r", response->data[1]);
+    if (response->handle == command_bleCharacteristic.getValueHandle())
+    {
+
+        // From CLIENT it writes a command in this format: Feature (4) + Type (1) + Data (2)        
+        uint32_t Feature = response->data[0]<<24 | response->data[1]<<16 | response->data[2]<<8 | response->data[3];
+        uint8_t Type = response->data[4];
+        int16_t Data = response->data[5]<<8 | response->data[6];
+            
+        //printf("\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);
 
-    // Return original value after inverted with STORE_LE_16 to send by BLE
-    value_read = response->data[0] + (response->data[1]<<8);
-
-    if (response->handle == userbutton_bleCharacteristic.getValueHandle())
-    {
-        printf("\r\n\r\nonDataWrite BUTTON (data %d)\r\n", value_read);
+        switch(Feature)
+        {
+            case FeatureStatus:
+                printf("\r\nSend STATUS %d\r\n", Data);
+                Status = Data;
+                trigger_Op = WRITE_DIF;
+                break;
+     
+            case FeatureDifference:
+                printf("\r\nSend DIFF %d\r\n", Data);
+                diff = Data;
+                trigger_Op = READ_STA;
+                break;
+ 
+            case FeaturePosition:
+                break;
+            
+            case FeatureSunPanel:
+                break;
+                
+            default:
+                break;
+        }
         
-        trigger_Op = READ_BUT; 
     }
-    
-    if (response->handle == difference_bleCharacteristic.getValueHandle())
-    {        
-        //printf("onDataWrite DIFF (data %d)\r\n", value_read);
-        
-        trigger_Op = READ_BUT;  
-    }
+
 }
 
 /** 
@@ -287,13 +354,14 @@
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
 {
-    //uint16_t CONTROL_SERVICE_UUID = 0xA000;
-    //uint16_t USERBUTTON_CHARACTERISTIC_UUID = 0xA001;
-
-    //uint16_t SENSORS_SERVICE_UUID = 0xB000;
-    //uint16_t DIFFERENCE_CHARACTERISTIC_UUID = 0xB001;
-    //uint16_t POSITION_CHARACTERISTIC_UUID = 0xB002;
-    //uint16_t SUNPANEL_CHARACTERISTIC_UUID = 0xB003;    
+    // Good only for Short UUID
+    //uint16_t CONTROL_SERVICE_UUID = 0x00,0x00,0x00,0x00,0x00,0x0F,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t COMMAND_CHARACTERISTIC_UUID = 0x00,0x00,0x00,0x02,0x00,0x0F,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t SENSORS_SERVICE_UUID = 0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t STATUS_CHARACTERISTIC_UUID = 0x00,0x00,0x04,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t DIFFERENCE_CHARACTERISTIC_UUID = 0x00,0x00,0x08,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t POSITION_CHARACTERISTIC_UUID = 0x00,0x00,0x10,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;
+    //uint16_t SUNPANEL_CHARACTERISTIC_UUID = 0x00,0x00,0x20,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b;    
     
     if (params->role == Gap::CENTRAL)
     {
@@ -303,7 +371,8 @@
         ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback);
         
         // Works only for one characteristic, if you need to discover all characteristic, use the above command
-        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, CONTROL_SERVICE_UUID, USERBUTTON_CHARACTERISTIC_UUID);  
+        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, CONTROL_SERVICE_UUID, COMMAND_CHARACTERISTIC_UUID); 
+        //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, STATUS_CHARACTERISTIC_UUID);  
         //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, DIFFERENCE_CHARACTERISTIC_UUID);
         //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, POSITION_CHARACTERISTIC_UUID);
         //ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, SENSORS_SERVICE_UUID, SUNPANEL_CHARACTERISTIC_UUID);
@@ -348,33 +417,34 @@
 
     ble.gap().setScanParams(SCAN_INT, SCAN_WIND);
     ble.gap().startScan(advertisementCallback);
-            
+    
     // Main Loop
-    while (true) {  
+    while (true)
+    {  
     
         //if (!ble.gattClient().isServiceDiscoveryActive())
         if (trigger_Op!=IDLE) // Wait until discoveryTerminationCallback
         {   
             
             // At present ARM BLE_API don't support Enable/Disable Notify from Client
-            //userbutton_bleCharacteristic.notify(); 
+            //command_bleCharacteristic.notify(); 
             //difference_bleCharacteristic.notify();
             //position_bleCharacteristic.notify();
             //sunpanel_bleCharacteristic.notify();
             
             // Don't run more ble .read or .write in the same loop
             // because the callback is an interrupt and it doesn't work correctly if overloaded
-          
-            if (display == 0 || display == 1)
-            { if (trigger_Op == WRITE_DIF) trigger_Op = READ_BUT; } // if Display 0 or 1, skip WRITE_DIF
+            
+            if (Status == 1 || Status == 2)
+            { if (trigger_Op == WRITE_DIF) trigger_Op = READ_STA; } // if Display 1 or 2, skip WRITE_DIF
             
-            if (display == 2)
-            { if (trigger_Op == READ_DIF) trigger_Op = WRITE_BUT; } // if Display 2, skip READ_DIF, READ_POS, READ_SUN
-            
+            if (Status == 3)
+            { if (trigger_Op == READ_DIF) trigger_Op = READ_POS; } // if Display 3, skip READ_DIF
+                        
             switch(trigger_Op)
             {
-            case READ_BUT:
-                userbutton_bleCharacteristic.read();
+            case READ_STA:
+                status_bleCharacteristic.read();
                 trigger_Op = IDLE;
                 break;
             case READ_DIF:
@@ -389,19 +459,27 @@
                 sunpanel_bleCharacteristic.read();
                 trigger_Op = IDLE;
                 break;
-            case WRITE_BUT:
+            case WRITE_STA:
                 if (trigger_button == true)
-                {
-                    memset (value_write, 0, SIZEOF_CONTROL_DATA_LEN); 
-                    STORE_LE_16(value_write, 1234);
-                    userbutton_bleCharacteristic.write(SIZEOF_CONTROL_DATA_LEN, value_write);
+                {                    
+                    memset (value_write, 0, COMMAND_DATA_LEN);
+                    //value_write = Feature (4) + Type (1) + Data (2)
+                    value_write[0] = (FeatureStatus & 0xff000000) >> 24;
+                    value_write[1] = (FeatureStatus & 0x00ff0000) >> 16;
+                    value_write[2] = (FeatureStatus & 0x0000ff00) >> 8;
+                    value_write[3] = (FeatureStatus & 0x000000ff);
+                    value_write[4] = (0x00);
+                    value_write[5] = (Status & 0xff00) >> 8;
+                    value_write[6] = (Status & 0x00ff);
+                    command_bleCharacteristic.write(COMMAND_DATA_LEN, value_write);
+                             
                     trigger_Op = IDLE;
                     trigger_button = false;
                 } else {
                     #ifdef Sensors
                     trigger_Op = WRITE_DIF;
                     #else
-                    trigger_Op = READ_BUT;
+                    trigger_Op = READ_STA;
                     #endif
                 }
                 break;                                 
@@ -410,13 +488,21 @@
                 accelerometer->Get_X_Axes(acc_data);
                 acc_diff = acc_data[0];
                 //printf("Send BLE Difference %d lux/mems\n\r", acc_diff);
-                 
-                memset (value_write_diff, 0, DIFFERENCE_DATA_LEN);
-                STORE_LE_16(value_write_diff, acc_diff);
-                difference_bleCharacteristic.write(DIFFERENCE_DATA_LEN, value_write_diff);
+                               
+                memset (value_write, 0, COMMAND_DATA_LEN); 
+                //value_write = Feature (4) + Type (1) + Data (2)
+                value_write[0] = (FeatureDifference & 0xff000000) >> 24;
+                value_write[1] = (FeatureDifference & 0x00ff0000) >> 16;
+                value_write[2] = (FeatureDifference & 0x0000ff00) >> 8;
+                value_write[3] = (FeatureDifference & 0x000000ff);
+                value_write[4] = (0x00);
+                value_write[5] = (acc_diff & 0xff00) >> 8;
+                value_write[6] = (acc_diff & 0x00ff);
+                command_bleCharacteristic.write(COMMAND_DATA_LEN, value_write);
+
                 trigger_Op = IDLE;
                 #else
-                trigger_Op = READ_BUT;
+                trigger_Op = READ_STA;
                 #endif
                 break;
             default:
@@ -425,7 +511,7 @@
             
         }
         ble.waitForEvent();
-    }
+    } 
 }
 
 /* Ticker --------------------------------------------------------------------*/
@@ -439,7 +525,7 @@
 
 int main(void)
 {
-    printf("\r\n\r\nSunTracker Remote Control\r\n\r\n");
+    printf("\r\n\r\nSunTracker Remote Control by Fabio Brembilla\r\n\r\n");
     
     #ifdef Sensors
     // Initializing Sensors Component
@@ -453,5 +539,6 @@
     
     mybutton.fall(&User_Button_Pressed);
     
-    BLE::Instance().init(bleInitComplete);
+    BLE::Instance().init(bleInitComplete);  
+    
 }