Diff: main.cpp

Branch:

iBeacon

Revision:

16:e86a91db0b72

Parent:

15:9870514aec92

Child:

17:18f4bf2a368a

--- a/main.cpp	Wed Aug 01 17:13:18 2018 +0200
+++ b/main.cpp	Wed Aug 01 17:37:57 2018 +0200
@@ -1,330 +1,438 @@
-/* 
- * aconno.de
- * Made by Jurica Resetar
- * Edited by Karlo Milicevic
- * Edited by Dominik Bartolovic
- * All right reserved 
- *
- */
-
-#include "mbed.h"
-#include "ble/BLE.h"
-#include "acd52832_bsp.h"
-#include "GapAdvertisingData.h"
-#include "Si7006A20.h"
-#include "LSM9DS1.h"
-#include "math.h"
-#include "nrf52_digital.h"
-#include "adc52832_common/utilities.h"
-#include "MPL115A1.h"
-#include "acd_nrf52_saadc.h"
-
-#define V0 0.47    /* In volts */
-#define TC 0.01    /* In volts */
-#define VCC (3.6)
-#define VALUE_TO_PERCENTAGE (100)
-#define WAKEUP_TIME_DELAY_MS (150)
-#define APPLICATION_ID (0xCF170059)
-
-#define ADC_REFERENCE    (3.6f)    /* adc reference voltage */
-#define ADC_RESOLUTION   (1024)    /* 10-bit adc            */
-
-#define I2C_DATA (p19)
-#define I2C_CLK  (p20)
-#define SPI_MISO (p5)
-#define SPI_MOSI (p3)
-#define SPI_SCLK (p4)
-
-#define DEBUG           (0)
-#define DEBUG_PRINT     (1)
-#define SLEEP_TIME      (0.150)          /* Sleep time in seconds */
-#define WAKE_UP_TIME    (0.150)          /* Awake time in ms */
-#define ADV_INTERVAL    (1000)            /* Advertising interval in ms */
-#define GO_TO_SLEEP     (0)              /* Sleep flag: 0 -> Device will not go to sleep, 1 -> Will go to sleep mode */
-#define CALIBRATION_STEPS (20)
-
-#define TX_POWER        (4)
-
-static NRF52_SAADC analogIn;
-static NRF52_DigitalOut lightPower(p28);
-static NRF52_DigitalOut temperaturePower(p31);
-static NRF52_DigitalOut shdn(p6);
-static NRF52_DigitalOut led(p23);
-static NRF52_DigitalOut power(p2);
-static NRF52_DigitalOut cs(p7);
-static Si7006   *si;
-static LSM9DS1  *mems;
-static SPI      *spi;
-static MPL115A1 *mpl115a1;
-
-#if DEBUG_PRINT
-    #include "nrf52_uart.h"
-    NRF52_UART serial = NRF52_UART(p25, p26, Baud9600);   //Tx, RX BaudRate
-    char buffer[256] = {0};
-    #define SEND(...) {uint8_t len = sprintf(buffer, __VA_ARGS__); serial.send(buffer, len);}
-    #define SENDN(...) {uint8_t len = sprintf(buffer "\n\r", __VA_ARGS__); serial.send(buffer, len);}
-#else
-    #define SEND(...);
-    #define SENDN(...);
-#endif
-
-static bool sleepFlag = true;
-
-static vector3_s memsAccelerometerInit;
-static vector3_s memsGyroscopeInit;
-static vector3_s memsMagnetometerInit;
-
-static BLE &ble = BLE::Instance();
-static GapAdvertisingData adv_data = GapAdvertisingData();
-
-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;
-        };
-    };
-};
-static advertising_packet advertisementPacket;
-
-void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
-    //  Restart Advertising on disconnection
-    BLE::Instance().gap().startAdvertising();
-}
-
-/**
- *  Function for waking the core up
- */
-void wakeMeUp(){
-    sleepFlag = false;
-}
-
-/**
- * Callback triggered when the ble initialization process has finished
- */
-void bleInitComplete(BLE::InitializationCompleteCallbackContext *params){
-    BLE&        ble   = params->ble;
-    ble_error_t error = params->error;
-
-    if (error != BLE_ERROR_NONE){
-        return;
-    }
-
-    /* Ensure that it is the default instance of BLE */
-    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE){
-        return;
-    }
-
-    ble.gap().onDisconnection(disconnectionCallback);
-
-    /* setup advertising */
-    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
-    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
-    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);    
-    ble.gap().setAdvertisingInterval(ADV_INTERVAL);
-    ble.gap().setTxPower(TX_POWER);        // Set TX power to TX_POWER
-    ble.gap().startAdvertising();    
-}
-
-float getLight(){
-    return ((float)analogIn.getData()[1])/ADC_RESOLUTION * VALUE_TO_PERCENTAGE;
-}
-
-float voltage2temp(float vOut){
-    return ((float)vOut - (float)V0)/((float)TC);
-}
-
-float getTemperature(){
-    return voltage2temp(((float)analogIn.getData()[2])/ADC_RESOLUTION * (float)VCC);
-}
-
-uint8_t getBattery(){
-    uint16_t batteryVoltage = analogIn.getData()[0];
-    
-    const uint16_t zero_percent_limit = 739;
-    const uint16_t onehundred_percent_limit = 810;
-    const uint16_t percentage_increments = 5;
-    uint8_t percentage;
-    
-    if (batteryVoltage < zero_percent_limit)
-    {
-        percentage = 0;
-    }
-    else if(batteryVoltage > onehundred_percent_limit)
-    {
-        percentage = 100;
-    }
-    else
-    {
-        batteryVoltage -= zero_percent_limit;
-        percentage = (batteryVoltage*100)/(onehundred_percent_limit - zero_percent_limit);
-        percentage = percentage/percentage_increments*percentage_increments;
-    }
-    
-    return percentage;
-}
-
-float getHumidity(){
-    float result;
-    si->getHumidity(&result);
-    return result;
-}
-
-void readGyroscope(vector3_s *gyroscopeData){
-    mems->readGyroscope((int16_t *)gyroscopeData);
-    *gyroscopeData -= memsGyroscopeInit;
-}
-
-void readAccelerometer(vector3_s *accelerometerData){
-    mems->readAccelerometer((int16_t *)accelerometerData);
-    *accelerometerData -= memsAccelerometerInit;
-}
-
-void readMagnetometer(vector3_s *magnetometerData){
-    mems->readMagnetometer((int16_t *)magnetometerData);
-    *magnetometerData -= memsMagnetometerInit;
-}
-
-void calibrateAccelerometer(){
-    vector3_s accelerometerData;
-    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
-        readAccelerometer(&accelerometerData);
-        memsAccelerometerInit += accelerometerData;
-    }
-    memsAccelerometerInit /= CALIBRATION_STEPS;
-}
-
-void calibrateGyroscope(){
-    vector3_s gyroscopeData;
-    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
-        readGyroscope(&gyroscopeData);
-        memsGyroscopeInit += gyroscopeData;
-    }
-    memsGyroscopeInit /= CALIBRATION_STEPS;
-}
-
-void calibrateMag(){
-    vector3_s magnetometerData;
-    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
-        readMagnetometer(&magnetometerData);
-        memsMagnetometerInit += magnetometerData;
-    }
-    memsMagnetometerInit /= CALIBRATION_STEPS;
-}
-
-void updateData(){
-    static uint8_t advertisementType = 0;
-    int16_t temp_acc[3];
-    
-    if(advertisementType < 1){
-        advertisementPacket.type = 0x00;
-        readGyroscope((vector3_s *)advertisementPacket.gyroscope);
-        readAccelerometer((vector3_s *)temp_acc);
-        readMagnetometer((vector3_s *)advertisementPacket.magnetometer);
-        advertisementPacket.acc_lsb_value = (0xF9E);//(0x3D80);
-        advertisementPacket.accelerometer[0] = temp_acc[1];
-        advertisementPacket.accelerometer[1] = temp_acc[0];
-        advertisementPacket.accelerometer[2] = temp_acc[2];
-        // ^--- That's in ug cuz MSB is 1
-    }
-    else{
-        analogIn.updateData();
-        
-        advertisementPacket.type = 0x01;
-        advertisementPacket.temperature = getTemperature();
-        advertisementPacket.light       = getLight();
-        advertisementPacket.humidity    = getHumidity();
-        advertisementPacket.pressure    = mpl115a1->getPressure();
-        advertisementPacket.battery     = getBattery();
-    }
-#if DEBUG == 0
-    if(++advertisementType > 2) advertisementType = 0;
-#endif
-    
-    adv_data = ble.getAdvertisingData();
-    adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
-    ble.setAdvertisingData(adv_data);
-}
-
-#if DEBUG
-void do_per()
-{
-    /*
-    SEND("T: %f\r\nL: %f\r\nH: %f\r\nP: %f\r\nB: %d\r\n", advertisementPacket.temperature,
-                                                          advertisementPacket.light,
-                                                          advertisementPacket.humidity,
-                                                          advertisementPacket.pressure,
-                                                          advertisementPacket.battery);
-    */
-    
-    SEND("G: %6d %6d %6d\r\nA: %6d %6d %6d\r\nM: %6d %6d %6d\r\n", advertisementPacket.gyroscope[0], advertisementPacket.gyroscope[1], advertisementPacket.gyroscope[2],
-                                                                   advertisementPacket.accelerometer[0], advertisementPacket.accelerometer[1], advertisementPacket.accelerometer[2],
-                                                                   advertisementPacket.magnetometer[0], advertisementPacket.magnetometer[1], advertisementPacket.magnetometer[2]);
-}
-#endif
-
-int main(){
-    power = 1;
-    wait_ms(WAKEUP_TIME_DELAY_MS);
-    temperaturePower = 1;
-    lightPower = 1;
-    shdn = 1; // Wake up the pressure sensor
-    analogIn.addChannel(9); // Set VDD  as source to SAADC
-    analogIn.addChannel(6); // Light
-    analogIn.addChannel(7); // Temp
-    analogIn.calibrate();
-
-    advertisementPacket.header = APPLICATION_ID;
-    
-    ble.init(bleInitComplete);
-    
-    I2C i2c(I2C_DATA, I2C_CLK);
-    si       = new Si7006(&i2c);
-    mems     = new LSM9DS1(&i2c);
-    spi      = new SPI(SPI_MOSI, SPI_MISO, SPI_SCLK);
-    mpl115a1 = new MPL115A1(*spi, cs);
-    
-    mems->startAccelerometer();
-    mems->startGyroscope();
-    mems->startMagnetometer();
-    
-    led = 1;
-    
-    Ticker ticker;
-    ticker.attach(wakeMeUp, SLEEP_TIME); // Wake the device up
-    
-#if DEBUG
-    Ticker per;
-    per.attach(do_per, 1);
-#endif
-
-    while(ble.hasInitialized() == false){
-        /* spin loop */
-    }
-    
-    while(true){
-        if (sleepFlag && GO_TO_SLEEP){
-            ble.gap().stopAdvertising();
-            sleep();
-            ble.waitForEvent();
-        }
-        else{
-            // I'm awake
-            updateData();
-            ble.gap().startAdvertising();
-            wait_ms(WAKE_UP_TIME);
-            sleepFlag = true;
-        }
-    }
-}
+/*
+ * aconno.de
+ * Made by Jurica Resetar
+ * Edited by Karlo Milicevic
+ * Edited by Dominik Bartolovic
+ * All right reserved
+ *
+ */
+
+#include "mbed.h"
+#include "ble/BLE.h"
+#include "acd52832_bsp.h"
+#include "GapAdvertisingData.h"
+#include "Si7006A20.h"
+#include "LSM9DS1.h"
+#include "math.h"
+#include "nrf52_digital.h"
+#include "adc52832_common/utilities.h"
+#include "MPL115A1.h"
+#include "acd_nrf52_saadc.h"
+#include "service.h"
+#include <events/mbed_events.h>
+
+#define V0 0.47    /* In volts */
+#define TC 0.01    /* In volts */
+#define VCC (3.6)
+#define VALUE_TO_PERCENTAGE (100)
+#define WAKEUP_TIME_DELAY_MS (150)
+#define APPLICATION_ID (0xCF170059)
+
+#define ADC_REFERENCE    (3.6f)    /* adc reference voltage */
+#define ADC_RESOLUTION   (1024)    /* 10-bit adc            */
+
+#define I2C_DATA (p19)
+#define I2C_CLK  (p20)
+#define SPI_MISO (p5)
+#define SPI_MOSI (p3)
+#define SPI_SCLK (p4)
+
+#define DEBUG           	(0)
+#define DEBUG_PRINT     	(1)
+#define SLEEP_TIME      	(0.150)          /* Sleep time in seconds 		*/
+#define WAKE_UP_TIME    	(0.150)          /* Awake time in ms 			*/
+#define ADV_INTERVAL    	(1000)           /* Advertising interval in ms 	*/
+#define GO_TO_SLEEP     	(0)
+/* Sleep flag: 0 -> Device will not go to sleep, 1 -> Will go to sleep mode */
+#define CALIBRATION_STEPS 	(20)
+#define TX_POWER_DB      	(4)
+#define INVERT_AXES			(0)
+
+uint8_t gConnected = 0;
+
+DigitalOut redLed(p22);
+
+static NRF52_SAADC analogIn;
+static NRF52_DigitalOut lightPower(p28);
+static NRF52_DigitalOut temperaturePower(p31);
+static NRF52_DigitalOut shdn(p6);
+static NRF52_DigitalOut led(p23);
+static NRF52_DigitalOut power(p2);
+static NRF52_DigitalOut cs(p7);
+static Si7006   *si;
+static LSM9DS1  *mems;
+static SPI      *spi;
+static MPL115A1 *mpl115a1;
+
+static EventQueue eventQueue(32 * EVENTS_EVENT_SIZE);
+uint8_t myMacAddress[6] = {};
+MACService *macServicePtr;
+
+#if DEBUG_PRINT
+    #include "SEGGER_RTT.h"
+    #define printf(...) SEGGER_RTT_printf(0, __VA_ARGS__)
+#else
+    #define printf(...)
+#endif
+
+static bool sleepFlag = true;
+
+static vector3_s memsAccelerometerInit;
+static vector3_s memsGyroscopeInit;
+static vector3_s memsMagnetometerInit;
+
+static GapAdvertisingData adv_data = GapAdvertisingData();
+
+typedef struct __attribute__((packed, aligned(1)))
+{
+  uint16_t appleID;
+  uint8_t  secondID;
+  uint8_t  DataSize;
+  uint8_t  UUID[18];
+  uint8_t  major;
+  uint8_t  minor;
+  int8_t   RSSI;
+}IBeaconMSD;
+
+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;
+        };
+    };
+};
+
+void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
+{
+    BLE &ble = context->ble;
+    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
+}
+
+static advertising_packet advertisementPacket;
+IBeaconMSD ibeaconMSD;
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+    //  Restart Advertising on disconnection
+    gConnected = 0;
+    BLE::Instance().gap().startAdvertising();
+}
+
+/**
+ *  Function for waking the core up
+ */
+void wakeMeUp()
+{
+    sleepFlag = false;
+}
+
+void onConnectionCallback(const Gap::ConnectionCallbackParams_t *params)
+{
+    printf("Connection callback.\n");
+    gConnected = 1;
+}
+
+/**
+ * Callback triggered when the ble initialization process has finished
+ */
+void bleInitCompleteIBeacon(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE&        ble   = params->ble;
+    ble_error_t error = params->error;
+
+    if (error != BLE_ERROR_NONE){
+        return;
+    }
+
+    /* Ensure that it is the default instance of BLE */
+    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE){
+        return;
+    }
+
+    uint8_t mac[6] = {0,0,0,0,0,0};
+    BLEProtocol::AddressType_t temp_address_type;
+    ble.gap().getAddress(&temp_address_type, myMacAddress);
+    macServicePtr = new MACService(ble, mac);
+    macServicePtr->updateMacAddress(myMacAddress);    // Update MAC address
+
+    ble.gap().onConnection(onConnectionCallback);
+    ble.gap().onDisconnection(disconnectionCallback);
+
+    /*
+     *  iBeacon format
+     */
+     ble.gap().accumulateAdvertisingPayload(
+        GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.gap().accumulateAdvertisingPayload(
+        GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
+        (uint8_t*)&ibeaconMSD, sizeof(ibeaconMSD));
+
+    ble.gap().setAdvertisingInterval(ADV_INTERVAL);
+	ble.gap().setTxPower(TX_POWER);        // Set TX power to TX_POWER_DB
+    ble.gap().startAdvertising();
+}
+
+/**
+ * Callback triggered when the ble initialization process has finished
+ */
+void bleInitCompleteSensors(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE&        ble   = params->ble;
+    ble_error_t error = params->error;
+
+    if (error != BLE_ERROR_NONE){
+        return;
+    }
+
+    /* Ensure that it is the default instance of BLE */
+    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE){
+        return;
+    }
+
+    uint8_t mac[6] = {0,0,0,0,0,0};
+    BLEProtocol::AddressType_t temp_address_type;
+    ble.gap().getAddress(&temp_address_type, myMacAddress);
+    macServicePtr = new MACService(ble, mac);
+    macServicePtr->updateMacAddress(myMacAddress);    // Update MAC address
+
+    ble.gap().onConnection(onConnectionCallback);
+    ble.gap().onDisconnection(disconnectionCallback);
+
+    /* setup advertising */
+    ble.gap().accumulateAdvertisingPayload(
+        GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.gap().accumulateAdvertisingPayload(
+        GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
+        (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
+    ble.gap().setAdvertisingType(
+        GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+	ble.gap().setAdvertisingInterval(ADV_INTERVAL);
+	ble.gap().setTxPower(TX_POWER);        // Set TX power to TX_POWER_DB
+    ble.gap().startAdvertising();
+}
+
+float getLight()
+{
+    return ((float)analogIn.getData()[1])/ADC_RESOLUTION * VALUE_TO_PERCENTAGE;
+}
+
+float voltage2temp(float vOut)
+{
+    return ((float)vOut - (float)V0)/((float)TC);
+}
+
+float getTemperature()
+{
+    return voltage2temp(((float)analogIn.getData()[2])/ADC_RESOLUTION * (float)VCC);
+}
+
+uint8_t getBattery()
+{
+    uint16_t batteryVoltage = analogIn.getData()[0];
+    const uint16_t zero_percent_limit = 739;
+    const uint16_t onehundred_percent_limit = 810;
+    const uint16_t percentage_increments = 5;
+    uint8_t percentage;
+
+    if (batteryVoltage < zero_percent_limit)
+    {
+        percentage = 0;
+    }
+    else if(batteryVoltage > onehundred_percent_limit)
+    {
+        percentage = 100;
+    }
+    else
+    {
+        batteryVoltage -= zero_percent_limit;
+        percentage = (batteryVoltage*100)/(onehundred_percent_limit - zero_percent_limit);
+        percentage = percentage/percentage_increments*percentage_increments;
+    }
+
+    return percentage;
+}
+
+float getHumidity()
+{
+    float result;
+    si->getHumidity(&result);
+    return result;
+}
+
+void readGyroscope(vector3_s *gyroscopeData)
+{
+    mems->readGyroscope((int16_t *)gyroscopeData);
+    *gyroscopeData -= memsGyroscopeInit;
+}
+
+void readAccelerometer(vector3_s *accelerometerData)
+{
+    mems->readAccelerometer((int16_t *)accelerometerData);
+    *accelerometerData -= memsAccelerometerInit;
+}
+
+void readMagnetometer(vector3_s *magnetometerData){
+    mems->readMagnetometer((int16_t *)magnetometerData);
+    *magnetometerData -= memsMagnetometerInit;
+}
+
+void calibrateAccelerometer(){
+    vector3_s accelerometerData;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter)
+    {
+        readAccelerometer(&accelerometerData);
+        memsAccelerometerInit += accelerometerData;
+    }
+    memsAccelerometerInit /= CALIBRATION_STEPS;
+}
+
+void calibrateGyroscope(){
+    vector3_s gyroscopeData;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter)
+    {
+        readGyroscope(&gyroscopeData);
+        memsGyroscopeInit += gyroscopeData;
+    }
+    memsGyroscopeInit /= CALIBRATION_STEPS;
+}
+
+void calibrateMag(){
+    vector3_s magnetometerData;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter)
+    {
+        readMagnetometer(&magnetometerData);
+        memsMagnetometerInit += magnetometerData;
+    }
+    memsMagnetometerInit /= CALIBRATION_STEPS;
+}
+
+void updateData(){
+    static uint8_t advertisementType = 0;
+    int16_t temp_acc[3];
+    BLE &ble = BLE::Instance();
+
+    if(!advertisementType && !gConnected)
+    {
+        printf("Sensor format 1.\n");
+        while(BLE_ERROR_NONE != ble.shutdown());
+        ble.init(bleInitCompleteSensors);
+        adv_data = ble.getAdvertisingData();
+        advertisementPacket.type = 0x00;
+        readGyroscope((vector3_s *)advertisementPacket.gyroscope);
+        readAccelerometer((vector3_s *)temp_acc);
+        readMagnetometer((vector3_s *)advertisementPacket.magnetometer);
+        advertisementPacket.acc_lsb_value = (0xF9E);
+		// ^--- That's in ug cuz MSB is 1
+		#if INVERT_AXES
+        	advertisementPacket.accelerometer[0] = temp_acc[1];
+        	advertisementPacket.accelerometer[1] = temp_acc[0];
+        	advertisementPacket.accelerometer[2] = temp_acc[2];
+        #endif
+
+        adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA,
+            (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
+        ble.setAdvertisingData(adv_data);
+    }
+    else if (advertisementType == 1 && !gConnected)
+    {
+        printf("Sensor format 2.\n");
+        analogIn.updateData();
+        adv_data = ble.getAdvertisingData();
+        advertisementPacket.type = 0x01;
+        advertisementPacket.temperature = getTemperature();
+        advertisementPacket.light       = getLight();
+        advertisementPacket.humidity    = getHumidity();
+        advertisementPacket.pressure    = mpl115a1->getPressure();
+        advertisementPacket.battery     = getBattery();
+
+        adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA,
+            (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
+        ble.setAdvertisingData(adv_data);
+    }
+
+    else if (!gConnected)
+    {
+        printf("Beacon format!\n");
+        while(BLE_ERROR_NONE != ble.shutdown());
+        ble.init(bleInitCompleteIBeacon);
+    }
+
+    if(++advertisementType > 2) advertisementType = 0;
+
+}
+
+void blinky()
+{
+    redLed = !redLed;
+}
+
+int main()
+{
+    printf("Main started.\n");
+
+    Thread bleT;
+
+    power = 1;
+    wait_ms(WAKEUP_TIME_DELAY_MS);
+    temperaturePower = 1;
+    lightPower = 1;
+    shdn = 1; // Wake up the pressure sensor
+    analogIn.addChannel(9); // Set VDD  as source to SAADC
+    analogIn.addChannel(6); // Light
+    analogIn.addChannel(7); // Temp
+    analogIn.calibrate();
+
+    advertisementPacket.header = APPLICATION_ID;
+
+    ibeaconMSD.appleID = 0x004C;
+    ibeaconMSD.secondID = 0x02;
+    ibeaconMSD.DataSize = 0x15;
+    ibeaconMSD.UUID[0] = 0x11;
+    ibeaconMSD.UUID[1] = 0x22;
+    ibeaconMSD.UUID[2] = 0x33;
+    ibeaconMSD.RSSI = -4;
+
+    BLE &ble = BLE::Instance();
+    ble.init(bleInitCompleteSensors);
+    //ble.init(bleInitCompleteIBeacon);
+    while(ble.hasInitialized() == false){
+        /* spin loop */
+    }
+    //bleT.start(callback(bleEventsC));
+    ble.onEventsToProcess(scheduleBleEventsProcessing);
+
+    I2C i2c(I2C_DATA, I2C_CLK);
+    si       = new Si7006(&i2c);
+    mems     = new LSM9DS1(&i2c);
+    spi      = new SPI(SPI_MOSI, SPI_MISO, SPI_SCLK);
+    mpl115a1 = new MPL115A1(*spi, cs);
+
+    mems->startAccelerometer();
+    mems->startGyroscope();
+    mems->startMagnetometer();
+
+    led = 1;
+
+    Ticker ticker;
+    ticker.attach(wakeMeUp, SLEEP_TIME); // Wake the device up
+
+    eventQueue.call_every(500, blinky);
+    eventQueue.call_every(1000, updateData);
+
+    // This call stops main thread
+    eventQueue.dispatch_forever();
+
+}