Revision 22:7dae8496b97c, committed 2018-09-13

Comitter:

jurica238814

Date:

Thu Sep 13 15:14:14 2018 +0200

Branch:

SimpleGATTExample

Parent:

19:2681edc2f2b9

Commit message:

LAUT example program

Changed in this revision

--- a/aconno_ble/aconno_ble.cpp	Thu Sep 13 12:20:35 2018 +0200
+++ b/aconno_ble/aconno_ble.cpp	Thu Sep 13 15:14:14 2018 +0200
@@ -8,16 +8,19 @@
 #include "tasks.h"
 #include "lizzy_service.h"
 #include "aconnoConfig.h"
+#include "bsp.h"
+
+const static char deviceName[] = "LAUT";
 
 init_lizzy_t init_lizzy = {
     .buzz = false,
     .leds = {false ,false, false},
-    .acc_lsb = LSB_VALUE,
+    .acc_lsb = 023,
     .acc_data = {0, 0, 0}
 };
+
 LizzyService *lizzy_service;
 
-
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     (lizzy_service->get_ble())->gap().startAdvertising();
@@ -29,36 +32,24 @@
 
 void onDataWrittenCallback(const GattWriteCallbackParams *params)
 {
-    if ((params->handle == lizzy_service->get_buzz_handle()) &&
+
+    if ((params->handle == lizzy_service->get_red_handle()) &&
         (params->len == 1))
     {
-        if ((uint8_t)true < *(params->data))
-            lizzy_service->set_buzz_state(true);
-
-        updateBuzzLedsT.signal_set(UPDATE_BUZZ_LEDS);
-    }
-    else if ((params->handle == lizzy_service->get_red_handle()) &&
-        (params->len == 1))
-    {
-        if ((uint8_t)true < *(params->data))
-            lizzy_service->set_red_state(true);
+        if (*(params->data))
+		{
+            lizzy_service->set_red_state(LED_ON);
+		}
 
         updateBuzzLedsT.signal_set(UPDATE_BUZZ_LEDS);
     }
     else if ((params->handle == lizzy_service->get_green_handle()) &&
         (params->len == 1))
     {
-        if ((uint8_t)true < *(params->data))
-            lizzy_service->set_green_state(true);
-
-        updateBuzzLedsT.signal_set(UPDATE_BUZZ_LEDS);
-    }
-    else if ((params->handle == lizzy_service->get_blue_handle()) &&
-        (params->len == 1))
-    {
-        if ((uint8_t)true < *(params->data))
-            lizzy_service->set_blue_state(true);
-
+        if (*(params->data))
+		{
+            lizzy_service->set_green_state(LED_ON);
+		}
         updateBuzzLedsT.signal_set(UPDATE_BUZZ_LEDS);
     }
 }
@@ -91,19 +82,16 @@
 
     advertisementPacket.header = APPLICATION_ID;
     advertisementPacket.type = 0x00;
-    advertisementPacket.gyroscope[0] = (int16_t)0;
-    advertisementPacket.gyroscope[1] = (int16_t)0;
-    advertisementPacket.gyroscope[2] = (int16_t)0;
-    advertisementPacket.accelerometer[0] = (int16_t)0;
-    advertisementPacket.accelerometer[1] = (int16_t)0;
-    advertisementPacket.accelerometer[2] = (int16_t)0;
-    advertisementPacket.magnetometer[0] = (int16_t)0;
-    advertisementPacket.magnetometer[1] = (int16_t)0;
-    advertisementPacket.magnetometer[2] = (int16_t)0;
 
     /* setup advertising */
-    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
-    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(uint8_t)*11);
+    ble.gap().accumulateAdvertisingPayload(
+		GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.gap().accumulateAdvertisingPayload(
+		GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
+		(uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
+	ble.gap().accumulateAdvertisingPayload(
+		GapAdvertisingData::COMPLETE_LOCAL_NAME,
+		(uint8_t *)deviceName, sizeof(deviceName));
     ble.gap().setAdvertisingInterval(ADV_INTERVAL_MS);
     ble.gap().startAdvertising();
 }

--- a/aconno_ble/aconno_ble.h	Thu Sep 13 12:20:35 2018 +0200
+++ b/aconno_ble/aconno_ble.h	Thu Sep 13 15:14:14 2018 +0200
@@ -14,30 +14,12 @@
 
 #define MSD_SIZE_b          (10)
 #define ADV_INTERVAL_MS     (1000)
-#define APPLICATION_ID      (0xCF170059) //(0xCF170059)
-
-#define LSB_VALUE           (0xFFFF)    // this is divided by 2^16 in app
+#define APPLICATION_ID      (0x9A780059) //(0xCF170059)
 
 /* Global variables and constants */
-
 struct __attribute__((packed, aligned(1))) advertising_packet{
     uint32_t header;
     uint8_t  type;
-    union{
-        struct{
-            int16_t gyroscope[3];
-            int16_t accelerometer[3];
-            int16_t magnetometer[3];
-            uint16_t acc_lsb_value;
-        };
-        struct{
-            float temperature;
-            float humidity;
-            float pressure;
-            float light;
-            uint8_t battery;
-        };
-    };
 };
 
 extern LizzyService *lizzy_service;

--- a/aconno_ble/lizzy_service.h	Thu Sep 13 12:20:35 2018 +0200
+++ b/aconno_ble/lizzy_service.h	Thu Sep 13 15:14:14 2018 +0200
@@ -16,111 +16,68 @@
     int16_t acc_data[NUM_AXIS];
 } init_lizzy_t;
 
-
 class LizzyService{
     public:
-        const static uint16_t LIZZY_SERVICE_UUID = 0xA000;
-        const static uint16_t BUZZ_UUID = 0xA001;
-        const static uint16_t RED_UUID = 0xA002;
-        const static uint16_t GREEN_UUID = 0xA003;
-        const static uint16_t BLUE_UUID = 0xA004;
-        const static uint16_t ACC_LSB_UUID = 0xA005;
-        const static uint16_t ACC_DATA_UUID = 0xA006;
+        const static uint16_t LIZZY_SERVICE_UUID = 0xAB00;
+        const static uint16_t RED_UUID = 0xAB02;
+        const static uint16_t GREEN_UUID = 0xAB03;
 
         LizzyService(BLEDevice &_ble, init_lizzy_t *init) :
               ble(_ble),
-              buzz(BUZZ_UUID, &(init->buzz), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-              red_led(RED_UUID, &(init->leds[0]), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-              green_led(GREEN_UUID, &(init->leds[1]), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-              blue_led(BLUE_UUID, &(init->leds[2]), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-              acc_lsb(ACC_LSB_UUID, &(init->acc_lsb), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
-              acc_data(ACC_DATA_UUID, init->acc_data, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
+              red_led(RED_UUID, &(init->leds[0]),
+			  		GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+              green_led(GREEN_UUID, &(init->leds[1]),
+			  		GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
         {
-            GattCharacteristic *charTable[] = {&buzz, &red_led, &green_led, &blue_led, &acc_lsb, &acc_data};     // Add characteristick in table
-            GattService AckService(LIZZY_SERVICE_UUID, charTable, sizeof(charTable)/sizeof(GattCharacteristic *));   
+			// Add characteristics in table
+            GattCharacteristic *charTable[] = {&red_led, &green_led};
+            GattService AckService(LIZZY_SERVICE_UUID, charTable,
+				sizeof(charTable)/sizeof(GattCharacteristic *));
             ble.gattServer().addService(AckService); // Add service in the BLE
         }
-        
+
         inline BLEDevice *get_ble(){
             return &ble;
         }
-        
+
         // Characteristic setters.
-        inline void set_buzz_state(bool new_state){
-            ble.gattServer().write(buzz.getValueHandle(), (uint8_t*)&new_state, sizeof(new_state));
-        }
         inline void set_red_state(bool new_state){
-            ble.gattServer().write(red_led.getValueHandle(), (uint8_t*)&new_state, sizeof(new_state));
+            ble.gattServer().write(red_led.getValueHandle(),
+				(uint8_t*)&new_state, sizeof(new_state));
         }
         inline void set_green_state(bool new_state){
-            ble.gattServer().write(green_led.getValueHandle(), (uint8_t*)&new_state, sizeof(new_state));
-        }
-        inline void set_blue_state(bool new_state){
-            ble.gattServer().write(blue_led.getValueHandle(), (uint8_t*)&new_state, sizeof(new_state));
-        }
-        
-        inline void set_acc_lsb(uint16_t new_value){
-            ble.gattServer().write(acc_lsb.getValueHandle(), (uint8_t*)&new_value, sizeof(new_value));
+            ble.gattServer().write(green_led.getValueHandle(),
+				(uint8_t*)&new_state, sizeof(new_state));
         }
-        inline void set_acc_data(int16_t *new_values){
-            ble.gattServer().write(acc_data.getValueHandle(), (uint8_t*)new_values, sizeof(int16_t)*NUM_AXIS);
-        }
-        
+
         // Characteristic getters.
-        inline bool get_buzz_state(){
-            bool tmp;
-            uint16_t size = sizeof(tmp);
-            ble.gattServer().read(buzz.getValueHandle(), (uint8_t*)&tmp, &size);
-            return tmp;
-        }
         inline bool get_red_state(){
             bool tmp;
             uint16_t size = sizeof(tmp);
-            ble.gattServer().read(red_led.getValueHandle(), (uint8_t*)&tmp, &size);
+            ble.gattServer().read(red_led.getValueHandle(), (uint8_t*)&tmp,
+					&size);
             return tmp;
         }
         inline bool get_green_state(){
             bool tmp;
             uint16_t size = sizeof(tmp);
-            ble.gattServer().read(green_led.getValueHandle(), (uint8_t*)&tmp, &size);
+            ble.gattServer().read(green_led.getValueHandle(), (uint8_t*)&tmp,
+					&size);
             return tmp;
         }
-        inline bool get_blue_state(){
-            bool tmp;
-            uint16_t size = sizeof(tmp);
-            ble.gattServer().read(blue_led.getValueHandle(), (uint8_t*)&tmp, &size);
-            return tmp;
-        }
-        
-        inline GattAttribute::Handle_t get_buzz_handle(){
-            return buzz.getValueHandle();
-        }
+
         inline GattAttribute::Handle_t get_red_handle(){
             return red_led.getValueHandle();
         }
         inline GattAttribute::Handle_t get_green_handle(){
             return green_led.getValueHandle();
         }
-        inline GattAttribute::Handle_t get_blue_handle(){
-            return blue_led.getValueHandle();
-        }
-        
-        inline GattAttribute::Handle_t get_acc_lsb_handle(){
-            return acc_lsb.getValueHandle();
-        }inline GattAttribute::Handle_t get_acc_data_handle(){
-            return acc_data.getValueHandle();
-        }
-        
+
     private:
         BLEDevice &ble;
         // Create new characteristics
-        ReadWriteGattCharacteristic<bool> buzz;
         ReadWriteGattCharacteristic<bool> red_led;
         ReadWriteGattCharacteristic<bool> green_led;
-        ReadWriteGattCharacteristic<bool> blue_led;
-        
-        ReadOnlyGattCharacteristic<uint16_t> acc_lsb;
-        ReadOnlyArrayGattCharacteristic<int16_t, NUM_AXIS> acc_data;
 };
 
-#endif //__LIZZY_SERVICE_H__
\ No newline at end of file
+#endif //__LIZZY_SERVICE_H__

--- a/bsp/bsp.cpp	Thu Sep 13 12:20:35 2018 +0200
+++ b/bsp/bsp.cpp	Thu Sep 13 15:14:14 2018 +0200
@@ -9,14 +9,11 @@
 #include "bsp.h"
 #include "aconnoConfig.h"
 
-lisPower_t lisPower = LIS_OFF;
-char memsI2CAddress = I2C_ADDRESS;
-DigitalOut lisPowerPin(ACC_POWER_PIN);
-InterruptIn lisInt1Pin(INT_PIN1);
-//InterruptIn INT2(INT_PIN2);
-I2C i2c(I2C_DATA,I2C_CLK);
+DigitalOut redLed(RED_LED_PIN);
+DigitalOut greenLed(GREEN_LED_PIN);
 
-void lisPowerCtrl(lisPower_t power)
+void bspInit()
 {
-	lisPowerPin = power;
+	redLed = LED_OFF;
+	greenLed = LED_OFF;
 }

--- a/bsp/bsp.h	Thu Sep 13 12:20:35 2018 +0200
+++ b/bsp/bsp.h	Thu Sep 13 15:14:14 2018 +0200
@@ -1,43 +1,22 @@
+/**
+ * Aconno.de
+ */
+
 #ifndef __BSP_H__
 #define __BSP_H__
 
 #include "aconnoConfig.h"
 #include "mbed.h"
 
-#define I2C_ADDRESS     (0b00110010)
+#define RED_LED_PIN         (p15)
+#define GREEN_LED_PIN       (p16)
 
-#if NANO_MODULE
-    #define I2C_DATA            (p2)
-    #define I2C_CLK             (p3)
-    #define INT_PIN1            (p10)
-    #define INT_PIN2            (p9)
-    #define ACC_POWER_PIN       (p13)
-    #define RED_LED_PIN         (p12)
-#else
-    #define I2C_DATA            (p20)
-    #define I2C_CLK             (p17)
-    #define INT_PIN1            (p16)
-    #define INT_PIN2            (p15)
-    #define ACC_POWER_PIN       (p11)
-    #define RED_LED_PIN         (p31)   //p22
-    #define GREEN_LED_PIN       (p2)
-    #define BLUE_LED_PIN        (p3)
-    #define BUZZER_PIN          (p18)
-#endif
+#define LED_OFF 	(0)		// INVERTED FOR DIGITAL PINS
+#define LED_ON		(1)
 
-enum lisPower_t{
-	LIS_ON = 0,
-	LIS_OFF
-};
-
-extern lisPower_t lisPower;
+extern DigitalOut redLed;
+extern DigitalOut greenLed;
 
-extern DigitalOut lisPowerPin;
-extern char memsI2CAddress;
-extern InterruptIn lisInt1Pin;
-//extern InterruptIn INT2();
-extern I2C i2c;
-
-void lisPowerCtrl(lisPower_t power);
+void bspInit();
 
 #endif //__BSP_H__

--- a/bsp/bspLed.h	Thu Sep 13 12:20:35 2018 +0200
+++ b/bsp/bspLed.h	Thu Sep 13 15:14:14 2018 +0200
@@ -3,20 +3,7 @@
 
 #include "aconnoConfig.h"
 
-#define LED_OFF 	(1)
-#define LED_ON		(0)
 
-extern DigitalOut redLed;
-extern DigitalOut greenLed;
-extern DigitalOut blueLed;
-
-#if NANO_MODULE
-    #define RED_LED_PIN         (p12)
-#else
-    #define RED_LED_PIN         (p31)   //p22
-    #define GREEN_LED_PIN       (p2)
-    #define BLUE_LED_PIN        (p3)
-#endif
 
 void bspLedInit();
 

--- a/config/aconnoConfig.h	Thu Sep 13 12:20:35 2018 +0200
+++ b/config/aconnoConfig.h	Thu Sep 13 15:14:14 2018 +0200
@@ -8,25 +8,14 @@
 
 #include "mbed.h"
 
-#define PRINT_ON_RTT        (1)
+#define PRINT_ON_RTT        (0)
 #define DEBUG_LED           (0)
 #define NANO_MODULE         (0)
-//#define LSB_VALUE           (192)
-
-#define BLE_ACTIVE_TIME_S   (0.4)
-#define POWER_UP_DELAY_MS   (200)
-
 
 #if !PRINT_ON_RTT
     #define printf(...)
 #endif
 
-#define INT1_THRESHOLD      (20u)
-#define INT1_DUR            (0x00)
-
-#define ACC_INT_SIG         (0x00001020)    // Acc interrupt signal
-#define DISABLE_BLE_SIG     (0x00001030)
-
 extern Thread bleT;
 extern Thread measureT;
 extern Thread updateServiceT;

--- a/source/main.cpp	Thu Sep 13 12:20:35 2018 +0200
+++ b/source/main.cpp	Thu Sep 13 15:14:14 2018 +0200
@@ -12,36 +12,18 @@
 #include "aconnoConfig.h"
 #include "aconno_ble.h"
 
-#if !defined(MBED_CPU_STATS_ENABLED) || !defined(DEVICE_LPTICKER) || !defined(DEVICE_SLEEP)
+#if !defined(MBED_CPU_STATS_ENABLED) || !defined(DEVICE_LPTICKER) || \
+	!defined(DEVICE_SLEEP)
 #error [NOT_SUPPORTED] Test not supported for statistics
 #endif
 
 EventQueue queue(32 * EVENTS_EVENT_SIZE);
 
-Thread uartT;
 Thread bleT;
-Thread measureT;
 Thread updateServiceT;
 Thread updateBuzzLedsT;
 Thread updateStatisticsT;
 
-void disableI2C(){
-    NRF_TWI0->ENABLE = 0;
-    NRF_TWI0->PSELSCL = 0xFFFFFFFF;
-    NRF_TWI1->ENABLE = 0;
-    NRF_TWI1->PSELSCL = 0xFFFFFFFF;
-    NRF_TWIM0->ENABLE = 0;
-    NRF_TWIM0->PSEL.SCL = 0x80000000;
-    NRF_TWIM0->PSEL.SDA = 0x80000000;
-    NRF_TWIM1->ENABLE = 0;
-    NRF_TWIM0->PSEL.SCL = 0x80000000;
-    NRF_TWIM0->PSEL.SDA = 0x80000000;
-    DigitalOut foo(I2C_DATA);
-    DigitalOut bar(I2C_CLK);
-    foo = 1;
-    bar = 1;
-}
-
 void printStatsF()
 {
     mbed_stats_cpu_t stats;
@@ -58,76 +40,20 @@
     queue.call(Callback<void()>(&ble, &BLE::processEvents));
 }
 
-Thread testThread;
-
-void testFunction()
-{
-	while(1)
-	{
-		printf("Test thread\n");
-		Thread::wait(1000);
-	}
-}
-
-void int1Handler(void)
-{
-    __disable_irq();
-	/*
-    untilSleep = ACTIVE_PERIOD / MEASURE_INTERVAL_MS;
-    measureT.signal_set(START_MEAS);
-	*/
-	mems.clearIntFlag();
-	redLed = !redLed;
-    __enable_irq();
-}
-
 int main()
 {
-
 	printf("Main program started.\r\n");
-
-	bspLedInit();
-
-    Lis2dh12 mems(&i2c, memsI2CAddress);
+	bspInit();
 
-    lisPowerCtrl((lisPower_t)LIS_ON);
-    wait_ms(POWER_UP_DELAY_MS);
-
-
-    lisInt1Pin.rise(int1Handler);
-    NRF_GPIO->PIN_CNF[INT_PIN1] &= 0xFFFFFFF3;  // NO PullUps
-
-	/*
     BLE &ble = BLE::Instance();
 	ble.onEventsToProcess(bleEventsProcessing);
     ble.init(bleInitComplete);
-	*/
-
-    //mems.setMode(LOW_POWER);
-    mems.setMode(HIGH_RES);
-    mems.enableAxes(X_axis);
-    mems.enableAxes(Y_axis);
-    mems.enableAxes(Z_axis);
-    mems.setODR(ODR_50Hz);
-    //mems.setScale(_16g);
-    mems.setScale(_2g);
-    mems.int1Setup(0x40);       // IntActivity 1 driven to INT1 pad
-    mems.int1Latch(0x01);       // Enable int1 latch
-    mems.int1Threshold(INT1_THRESHOLD);
-    mems.int1Duration(INT1_DUR);
-    mems.int1Config(0x2A);      // Enable XHigh, YHigh and ZHigh
-	//lisPowerCtrl(LIS_OFF);
-
-	//disableI2C();		// THIS REALLY NEEDS TO BE HERE!
 
 	#if PRINT_ON_RTT
 		queue.call_every(2000, printStatsF);
 	#endif
-	//testThread.start(testFunction);
-    //measureT.start(callback(measureF, &mems));
-    //updateServiceT.start(updateServiceF);
-    //updateBuzzLedsT.start(updateBuzzLedsF);
 
+    updateBuzzLedsT.start(updateBuzzLedsF);
 	queue.dispatch_forever();
 
 }

--- a/tasks/tasks.cpp	Thu Sep 13 12:20:35 2018 +0200
+++ b/tasks/tasks.cpp	Thu Sep 13 15:14:14 2018 +0200
@@ -2,7 +2,6 @@
  * Aconno.de
  */
 
-#include "bsp_buzz.h"
 #include "bspLed.h"
 #include "aconno_ble.h"
 #include "tasks.h"
@@ -10,158 +9,32 @@
 #include "aconnoConfig.h"
 #include "bsp.h"
 
-Buzz buzzer(NRF_PWM2, BUZZER_PIN);
+#define SHORT_DELAY_MS			(75)
 
 static advertising_packet advertisementPacket;
 static GapAdvertisingData adv_data = GapAdvertisingData();
 
-
-#if TEST_LEDS_BUZZ
-Ticker test_ticker;
-#endif
-
-
-void tasks_init()
+void updateBuzzLedsF()
 {
 
-#if TEST_LEDS_BUZZ
-    test_ticker.attach(led_tick, 0.5);
-#endif
-}
-
-#if TEST_LEDS_BUZZ
-void buzz_tick()
-{
-    static int start = 1;
-
-    if (start)
-    {
-        buzzer.enable();
-        start = 0;
-    }
-    else
-    {
-        buzzer.disable();
-        start = 1;
-        led_tick();
-        test_ticker.detach();
-        test_ticker.attach(led_tick, 0.5);
-    }
-}
-
-void led_tick()
-{
-    static int count = 0;
-
-    switch(count)
-    {
-        case 0:
-            redLed = 0;
-            break;
-
-        case 1:
-            redLed = 1;
-            greenLed = 0;
-            break;
-
-        case 2:
-            greenLed = 1;
-            blueLed = 0;
-            break;
-
-        default:
-            blueLed = 1;
-            count = -1;
-            buzz_tick();
-            test_ticker.detach();
-            test_ticker.attach(buzz_tick, BUZZ_TIME_S);
-    }
-
-    count++;
-}
-#endif
-
-void measureF(Lis2dh12 *mems)
-{
-    while (1)
-    {
-        advertisementPacket.header = APPLICATION_ID;
-        advertisementPacket.type = 0x00;
-        advertisementPacket.gyroscope[0] = (int16_t)0;
-        advertisementPacket.gyroscope[1] = (int16_t)0;
-        advertisementPacket.gyroscope[2] = (int16_t)0;
-        advertisementPacket.magnetometer[0] = (int16_t)0;
-        advertisementPacket.magnetometer[1] = (int16_t)0;
-        advertisementPacket.magnetometer[2] = (int16_t)0;
-
-		lisPowerCtrl(LIS_ON);
-		wait_ms(POWER_UP_DELAY_MS);
-		/*
-		//mems.setMode(LOW_POWER);
-	    mems.setMode(HIGH_RES);
-	    mems.enableAxes(X_axis);
-	    mems.enableAxes(Y_axis);
-	    mems.enableAxes(Z_axis);
-	    mems.setODR(ODR_50Hz);
-	    //mems.setScale(_16g);
-	    mems.setScale(_2g);
-	    mems.int1Setup(0x40);       // IntActivity 1 driven to INT1 pad
-	    mems.int1Latch(0x01);       // Enable int1 latch
-	    mems.int1Threshold(INT1_THRESHOLD);
-	    mems.int1Duration(INT1_DUR);
-	    mems.int1Config(0x2A);      // Enable XHigh, YHigh and ZHigh
-		*/
-
-        advertisementPacket.accelerometer[0] = (int16_t)mems->readXAxis();
-        advertisementPacket.accelerometer[1] = (int16_t)mems->readYAxis();
-        advertisementPacket.accelerometer[2] = (int16_t)mems->readZAxis();
-
-        advertisementPacket.acc_lsb_value = LSB_VALUE;
-
-        updateServiceT.signal_set(MEAS_DONE);
-        bleT.signal_set(MEAS_DONE);
-
-		lisPowerCtrl(LIS_OFF);
-        Thread::wait(MEASURE_INTERVAL_MS);
-    }
-}
-
-void updateServiceF()
-{
-    while (1)
-    {
-        Thread::signal_wait(MEAS_DONE);
-        updateServiceT.signal_clr(MEAS_DONE);
-
-        lizzy_service->set_acc_data(advertisementPacket.accelerometer);
-    }
-}
-
-void updateBuzzLedsF()
-{
-    while (1)
+    while (true)
     {
         Thread::signal_wait(UPDATE_BUZZ_LEDS);
         updateBuzzLedsT.signal_clr(UPDATE_BUZZ_LEDS);
 
-        if (buzzer.get_state() != (lizzy_service->get_buzz_state()))
-        {
-            if (lizzy_service->get_buzz_state())
-                buzzer.enable();
-            else
-                buzzer.disable();
-        }
-        if (!redLed != (lizzy_service->get_red_state()))
+        if (lizzy_service->get_red_state())
         {
-            redLed = !(lizzy_service->get_red_state());
+            redLed = LED_ON;
+			wait_ms(SHORT_DELAY_MS);
+			redLed = LED_OFF;
+			lizzy_service->set_red_state(LED_OFF);
         }
-        if (!greenLed != (lizzy_service->get_green_state()))
+        if (lizzy_service->get_green_state())
         {
-            greenLed = !(lizzy_service->get_green_state());
-        }
-        if (!blueLed != (lizzy_service->get_blue_state()))
-        {
-            blueLed = !(lizzy_service->get_blue_state());
+            greenLed = LED_ON;
+			wait_ms(SHORT_DELAY_MS);
+			greenLed = LED_OFF;
+			lizzy_service->set_green_state(LED_OFF);
         }
     }
 }
@@ -173,50 +46,9 @@
         Thread::signal_wait(MEAS_DONE);
         bleT.signal_clr(MEAS_DONE);
 
-        /*
-        printf("%6d\t", advertisementPacket.accelerometer[0]);
-        printf("%6d\t", advertisementPacket.accelerometer[1]);
-        printf("%6d\r\n", advertisementPacket.accelerometer[2]);
-        */
-
-        //updatePayload(ble, &advertisementPacket);
-
-        /* setup advertising */
-
-
-        /*
-        GapAdvertisingData advetisementData = GapAdvertisingData();
-        advetisementData = ble->getAdvertisingData();
-        advetisementData.updateData(advetisementData.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertising_packet));
-        ble->setAdvertisingData(advetisementData);
-        */
         adv_data = ble->getAdvertisingData();
-        adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
+        adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA,
+			(uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
         ble->setAdvertisingData(adv_data);
-
-
-        //printf("Ble advertisement is ON.\r\n");
-
-        #if DEBUG_LED
-            redLed = 0;
-        #endif
-        //ble->gap().startAdvertising();
-        //wait_ms(1000);
-
-        //wait_ms(MEASURE_INTERVAL_MS);
-        //wait_ms(1000);
-
-        //printf("Ble advertisement is OFF.\r\n\r\n");
-        #if DEBUG_LED
-            redLed = 1;
-        #endif
-        //ble->gap().stopAdvertising();
-        /*
-        GapAdvertisingData advetisementData = GapAdvertisingData();
-        advetisementData = ble->getAdvertisingData();
-        advetisementData.updateData(advetisementData.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertising_packet));
-        ble->setAdvertisingData(advetisementData);
-        */
-        //wait_ms(100);
     }
 }