aconno acnsensa project for iOS devices with iBeacon packets support.

Dependencies:   LSM9DS1 Si7006A20 aconno_SEGGER_RTT aconno_bsp adc52832_common

Revision:
1:326ce5e200fb
Parent:
0:12899fa39f88
Child:
2:c0654c5fb771
diff -r 12899fa39f88 -r 326ce5e200fb main.cpp
--- a/main.cpp	Mon Nov 27 12:12:59 2017 +0000
+++ b/main.cpp	Wed Dec 13 09:37:21 2017 +0000
@@ -1,6 +1,7 @@
 /* 
  * aconno.de
  * Made by Jurica Resetar
+ * Edited by Karlo Milicevic
  * All right reserved 
  *
  */
@@ -13,106 +14,97 @@
 #include "LSM9DS1.h"
 #include "math.h"
 #include "nrf52_digital.h"
+#include "adc52832_common/utilities.h"
+#include "MPL115A1.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 I2C_DATA (p19)
+#define I2C_CLK  (p20)
+#define SPI_MISO (p5)
+#define SPI_MOSI (p3)
+#define SPI_SCLK (p4)
 
 #define DEBUG_PRINT     (0)
 #define SLEEP_TIME      (0.150)          /* Sleep time in seconds */
 #define WAKE_UP_TIME    (0.150)          /* Awake time in ms */
-#define MSD_SIZE        (25)             /* Manufacturer Specific Data lenght (in B) */
 #define ADV_INTERVAL    (100)            /* Advertising interval in ms */
-#define TX_POWER        (4)              /* TX power (in dB) */
 #define GO_TO_SLEEP     (0)              /* Sleep flag: 0 -> Device will not go to sleep, 1 -> Will go to sleep mode */
+#define CALIBRATION_STEPS 20
 
-// Definitions of the location within MSD for the data
-#define ADV_TYPE    (4)
-#define GYRO        (5)
-#define ACC         (11)
-#define MAGNETO     (17)
-#define TEMPERATURE (5)
-#define HUMIDITY    (9)
-#define PRESSURE    (13)
-#define LIGHT       (17)
-
-
-AnalogIn temperature(ADC_TEMP);
-AnalogIn light(ADC_LIGHT);
-NRF52_DigitalOut lightPower(p28);
-NRF52_DigitalOut temperaturePower(p31);
-SPI spi(p3, p5, p4); // mosi, miso, sclk
-NRF52_DigitalOut cs(p7);
-NRF52_DigitalOut shdn(p6);
-NRF52_DigitalOut led(p23);
-NRF52_DigitalOut power(p2);
-Si7006 *si;
-LSM9DS1 *mems;
+static AnalogIn temperature(ADC_TEMP);
+static AnalogIn light(ADC_LIGHT);
+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
-    uint8_t charCounter;
+    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
 
-bool SLEEP = true;
-int8_t txPower = 4;
+static bool sleepFlag = true;
 
-union float2bytes{ 
-    float f; 
-    char b[sizeof(float)]; 
-};
+static vector3_s memsAccInit;
+static vector3_s memsGyrInit;
+static vector3_s memsMagInit;
 
-float2bytes temp2byte;
-float2bytes hum2byte;
-float2bytes light2byte;
-float2bytes pressure2byte;
+static BLE &ble = BLE::Instance();
+static GapAdvertisingData adv_data = GapAdvertisingData();
 
-uint16_t a0_frac_mask = 0x0007;
-uint8_t a0_frac_bits = 3;
-uint16_t b1_frac_mask = 0x1FFF;
-uint8_t b1_frac_bits = 13;
-uint16_t b2_frac_mask = 0x3FFF;
-uint8_t b2_frac_bits = 14;
-uint16_t c12_frac_mask = 0x1FFF;
-uint8_t c12_frac_bits = 22;
+struct __attribute__((packed, aligned(1))) advertising_packet{
+    uint32_t header;
+    uint8_t  type;
+    union{
+        struct{
+            int16_t gyr[3];
+            int16_t acc[3];
+            int16_t mag[3];
+        };
+        struct{
+            float temperature;
+            float humidity;
+            float pressure;
+            float light;
+        };
+    };
+};
+static advertising_packet advPacket;
 
-int16_t memsBuffer[3];
-int16_t memsAccInit[3]={0,0,0};
-int16_t memsGyroInit[3]={0,0,0};
-int16_t memsMagInit[3]={0,0,0};
-
-BLE &ble = BLE::Instance();
-GapAdvertisingData adv_data = GapAdvertisingData();
-uint8_t MSD[MSD_SIZE] = {0x59, 0x00, 0x17, 0xCF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-
-
-/**
- * Restart Advertising on disconnection
- */
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
+    //  Restart Advertising on disconnection
     BLE::Instance().gap().startAdvertising();
 }
 
 /**
  *  Function for waking the core up
  */
-void wakeMeUp(void){
-        SLEEP = false;
-    }
+void wakeMeUp(){
+    sleepFlag = false;
+}
 
-/**
- * This function is called when the ble initialization process has failed
- */
 void onBleInitError(BLE &ble, ble_error_t error){
     /* Avoid compiler warnings */
     (void) ble;
     (void) error;
-    /* Initialization error handling should go here */
 }
 
 /**
@@ -123,7 +115,6 @@
     ble_error_t error = params->error;
 
     if (error != BLE_ERROR_NONE) {
-        /* In case of error, forward the error handling to onBleInitError */
         onBleInitError(ble, error);
         return;
     }
@@ -137,429 +128,112 @@
 
     /* setup advertising */
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
-    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)MSD, MSD_SIZE);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advPacket, sizeof(advPacket));
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);    
     ble.gap().setAdvertisingInterval(ADV_INTERVAL);
     ble.gap().startAdvertising();    
 }
 
-
-
-void getLight(uint8_t *light_data){
-    light2byte.f = light.read() * 100;
-    *(light_data+0) = light2byte.b[0];
-    *(light_data+1) = light2byte.b[1];
-    *(light_data+2) = light2byte.b[2];
-    *(light_data+3) = light2byte.b[3];
+float getLight(){
+    return light.read() * VALUE_TO_PERCENTAGE;
 }
 
 float voltage2temp(float vOut){
     return ((float)vOut - (float)V0)/((float)TC);
 }
 
-void getTemperature(uint8_t *temperature_data){
-    temp2byte.f = voltage2temp(temperature.read()*(float)VCC);
-    *(temperature_data+0) = temp2byte.b[0];
-    *(temperature_data+1) = temp2byte.b[1];
-    *(temperature_data+2) = temp2byte.b[2];
-    *(temperature_data+3) = temp2byte.b[3];    
+float getTemperature(){
+    return voltage2temp(temperature.read()*(float)VCC);
 }
 
-void getHumidity(uint8_t *hum_data){
-    //float temp;
-    float humid;
-    si->getHumidity(&humid);
-    //si.getTemperature(&temp);
-    hum2byte.f = humid;
-    *(hum_data+0) = hum2byte.b[0];
-    *(hum_data+1) = hum2byte.b[1];
-    *(hum_data+2) = hum2byte.b[2];
-    *(hum_data+3) = hum2byte.b[3];
+float getHumidity(){
+    float result;
+    si->getHumidity(&result);
+    return result;
 }
 
-void readGyro(int8_t *gyro_data){
-    mems->readGyro(memsBuffer); 
-    // Use init data values
-    memsBuffer[0] -= memsGyroInit[0];
-    memsBuffer[1] -= memsGyroInit[1];
-    memsBuffer[2] -= memsGyroInit[2];   
-    gyro_data[0] = memsBuffer[0] & 0xFF;   // X axis
-    gyro_data[1] = (memsBuffer[0] >> 8);   // X axis
-    gyro_data[2] = memsBuffer[1] & 0xFF;   // Y axis
-    gyro_data[3] = (memsBuffer[1] >> 8);   // Y axis
-    gyro_data[4] = memsBuffer[2] & 0xFF;   // Z axis
-    gyro_data[5] = (memsBuffer[2] >> 8);   // Z axis   
+void readGyr(vector3_s *gyro_data){
+    mems->readGyro((int16_t *)gyro_data);
+    *gyro_data -= memsGyrInit;
 }
 
-void readMyAcc(int8_t *acc_data){
-    mems->readAcc(memsBuffer);
-    // Use init data values
-    memsBuffer[0] -= memsAccInit[0];
-    memsBuffer[1] -= memsAccInit[1];
-    memsBuffer[2] -= memsAccInit[2];
-    acc_data[0] = memsBuffer[0] & 0xFF;     // X axis
-    acc_data[1] = (memsBuffer[0] >> 8);     // X axis
-    acc_data[2] = memsBuffer[1] & 0xFF;     // Y axis
-    acc_data[3] = (memsBuffer[1] >> 8);     // Y axis
-    acc_data[4] = memsBuffer[2] & 0xFF;     // Z axis
-    acc_data[5] = (memsBuffer[2] >> 8);     // Z axis
+void readAcc(vector3_s *acc_data){
+    mems->readAcc((int16_t *)acc_data);
+    *acc_data -= memsAccInit;
 }
 
-void readMagneto(int8_t *magneto_data){
-    mems->readMag(memsBuffer);
-    // Use init data values
-    memsBuffer[0] -= memsMagInit[0];
-    memsBuffer[1] -= memsMagInit[1];
-    memsBuffer[2] -= memsMagInit[2];
-    magneto_data[0] = memsBuffer[0] * 0xFF; // X axis
-    magneto_data[1] = (memsBuffer[0] >> 8); // X axis
-    magneto_data[2] = memsBuffer[1] * 0xFF; // X axis
-    magneto_data[3] = (memsBuffer[1] >> 8); // X axis
-    magneto_data[4] = memsBuffer[2] * 0xFF; // X axis
-    magneto_data[5] = (memsBuffer[2] >> 8); // X axis
+void readMag(vector3_s *magneto_data){
+    mems->readMag((int16_t *)magneto_data);
+    *magneto_data -= memsMagInit;
 }
 
 void calibrateAcc(){
-    uint8_t counter=0;
-    int8_t acc_data[6];
-    for(counter=0; counter<20; counter++){
-        readMyAcc(acc_data);
-        memsAccInit[0] += acc_data[0];
-        memsAccInit[1] += acc_data[1];
-        memsAccInit[2] += acc_data[2];
+    vector3_s acc_data;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
+        readAcc(&acc_data);
+        memsAccInit += acc_data;
     }
-    memsAccInit[0] /= counter;
-    memsAccInit[1] /= counter;
-    memsAccInit[2] /= counter;
+    memsAccInit /= CALIBRATION_STEPS;
 }
 
-void calibrateGyro(){
-    uint8_t counter;
-    int8_t gyro_data[6];
-    for(counter=0; counter<20; counter++){
-        readMyAcc(gyro_data);
-        memsGyroInit[0] += gyro_data[0];
-        memsGyroInit[1] += gyro_data[1];
-        memsGyroInit[2] += gyro_data[2];
+void calibrateGyr(){
+    vector3_s gyro_data;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
+        readGyr(&gyro_data);
+        memsGyrInit += gyro_data;
     }
-    memsGyroInit[0] /= counter;
-    memsGyroInit[1] /= counter;
-    memsGyroInit[2] /= counter;
+    memsGyrInit /= CALIBRATION_STEPS;
 }
 
 void calibrateMag(){
-    uint8_t counter=0;
-    int8_t mag_data[6];
-    for(counter=0; counter<20; counter++){
-        readMyAcc(mag_data);
-        memsMagInit[0] += mag_data[0];
-        memsMagInit[1] += mag_data[1];
-        memsMagInit[2] += mag_data[2];
-    }
-    memsMagInit[0] /= counter;
-    memsMagInit[1] /= counter;
-    memsMagInit[2] /= counter;
-}
-
-
-float get_a0(uint16_t a0){
-    float temp_a0;
-    float temp_frac;
-    int temp_a0_int;
-    uint8_t negative_flag = 0;
-    
-    if(a0& 0x8000){
-        a0 ^= 0xFFFF;   // Transform from 2's complement
-        a0 -= 1;
-        negative_flag = 1;
-    }
-    
-    temp_a0_int = a0 & 0x7FF8;
-    temp_a0_int = temp_a0_int >> a0_frac_bits;
-    
-    temp_a0 = temp_a0_int * 1.0;    // Int part
-    temp_frac = (1.0/(pow(2.0,3)));
-    temp_frac *= (a0 & a0_frac_mask);
-    temp_a0 = temp_a0 + temp_frac;
-    
-    if(negative_flag) temp_a0 = -temp_a0;
-    
-    return temp_a0;
-}
-
-float get_b1(uint16_t b1){
-    float temp_b1;
-    float temp_frac;
-    int temp_b1_int;
-    uint8_t negative_flag = 0;
-    
-    if (b1 & 0x8000){
-        b1 ^= 0xFFFF;
-        b1 -= 1;
-        negative_flag = 1;
-    }
-    
-    temp_b1_int = b1 & 0x6000;
-    temp_b1_int = temp_b1_int >> b1_frac_bits;
-    
-    temp_b1 = temp_b1_int * 1.0;
-    temp_frac = (b1 & b1_frac_mask) * (1.0/(pow(2.0,b1_frac_bits)));
-    temp_b1 = temp_b1 + temp_frac;
-    
-    if (negative_flag) temp_b1 = -temp_b1;
-    
-    return temp_b1;
-}
-
-float get_b2(uint16_t b2){
-    float temp_b2;
-    float temp_frac;
-    int temp_b2_int;
-    uint8_t negative_flag = 0;
-    
-    if (b2 & 0x8000){
-        b2 ^= 0xFFFF;
-        b2 -= 1;
-        negative_flag = 1;
-    }
-    
-    temp_b2_int = b2 & 0x4000;
-    temp_b2_int = temp_b2_int >> b2_frac_bits;
-    
-    temp_b2 = temp_b2_int * 1.0;
-    temp_frac = (b2 & b2_frac_mask) * (1.0/(pow(2.0,b2_frac_bits)));
-    temp_b2 = temp_b2 + temp_frac;
-    
-    if (negative_flag) temp_b2 = -temp_b2;
-    
-    return temp_b2;
-}
-
-
-float get_c12(uint16_t c12){
-    float temp_c12;
-    float temp_frac;
-    uint8_t negative_flag = 0;
-    
-    c12 = c12 >> 2;
-    
-    if (c12 & 0x2000){
-        c12 ^= 0xFFFF;
-        c12 -= 1;
-        negative_flag = 1;
+    vector3_s mag_data;
+    for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
+        readMag(&mag_data);
+        memsMagInit += mag_data;
     }
-    
-    temp_c12 = 0.000000000;
-    temp_frac = (c12 & c12_frac_mask) * (1.0/(pow(2.0,c12_frac_bits)));
-    temp_c12 = temp_c12 + temp_frac;
-    
-    if (negative_flag) temp_c12 = -temp_c12;
-    
-    return temp_c12;
-}
-
-void get_pressure(uint8_t *pressure_ret){
-    float pcomp = 0x00;
-    volatile float pressure;
-
-    uint16_t a0 = 0;
-    uint16_t b1 = 0;
-    uint16_t b2 = 0;
-    uint16_t c12 = 0;
-    uint16_t padc = 0;
-    uint16_t tadc = 0;
-
-    float a0_f, b1_f, b2_f, c12_f;
-    cs = 0;
-    spi.write(0x88);        // MSB a0
-    a0 = spi.write(0x00);   
-    a0 = a0 << 8;
-    wait_ms(1); 
-    spi.write(0x8A);        // LSB a0
-    a0 |= spi.write(0x00);
-    wait_ms(1);
-    
-    spi.write(0x8C);        // MSB b1
-    b1 = spi.write(0x00);   
-    b1 = b1 << 8;
-    wait_ms(1);
-    spi.write(0x8E);        // LSB b1
-    b1 |= spi.write(0x00);
-    wait_ms(1);
-    
-    spi.write(0x90);        // MSB b2
-    b2 = spi.write(0x00);   
-    b2 = b2 << 8;
-    wait_ms(1);
-    spi.write(0x92);        // LSB b2
-    b2 |= spi.write(0x00);
-    wait_ms(1);
-    
-    spi.write(0x94);        // MSB c12
-    c12 = spi.write(0x00);  
-    c12 = c12 << 8;
-    wait_ms(1);
-    spi.write(0x96);        // LSB c12
-    c12 |= spi.write(0x00);
-    wait_ms(1);
-    spi.write(0x00);
-    cs = 1;
-    
-    cs = 0;
-    spi.write(0x24);        // Start conversion
-    spi.write(0x00);
-    cs = 1;
-    wait_ms(3);
-    
-    cs = 0;
-    spi.write(0x80);
-    padc = spi.write(0x00); // MSB Padc
-    padc = padc << 8;
-    spi.write(0x82);
-    padc |= spi.write(0x00);    // LSB Padc
-    padc = padc >> 6;
-    
-    spi.write(0x84);                
-    tadc = spi.write(0x00); // MSB Padc
-    tadc = tadc << 8;
-    spi.write(0x86);
-    tadc |= spi.write(0x00);    // LSB Padc
-    tadc = tadc >> 6;
-    
-    spi.write(0x00);
-    cs = 1;
-    
-    a0_f = get_a0(a0);
-    b1_f = get_b1(b1);
-    b2_f = get_b2(b2);
-    c12_f = get_c12(c12);
-    /*            
-    float c12x2 = c12_f * tadc;
-    float a1_f = b1_f + c12x2;
-    float a1x1 = a1_f * padc;
-    float y1 = a0_f + a1x1;
-    float a2x2 = b2_f * tadc;
-    pcomp = y1 + a2x2;
-    */
-
-    pcomp = a0_f + (b1_f + c12_f * tadc) * padc + b2_f * tadc;
-    pressure = pcomp * ((115-50)/(1023.0)) + 52;    // Was + 50
-    pressure *= 10;     // Calculate in hPa
-    pressure2byte.f = pressure;
-    *(pressure_ret+0) = pressure2byte.b[0];
-    *(pressure_ret+1) = pressure2byte.b[1];
-    *(pressure_ret+2) = pressure2byte.b[2];
-    *(pressure_ret+3) = pressure2byte.b[3];
+    memsMagInit /= CALIBRATION_STEPS;
 }
 
 void updateData(){
-    uint8_t temperature_data[4];
-    uint8_t light_data[4];
-    int8_t gyro_data[6];
-    int8_t acc_data[6];
-    int8_t magneto_data[6];
-    uint8_t hum_data[4];
-    uint8_t pressure_data[4];
     static uint8_t adv_type = 0;
     
-    if(adv_type < 1){   
-        
-        // Set adv type
-        MSD[ADV_TYPE] = 0x00;        
-        // Read gyro
-        readGyro(gyro_data);
-        MSD[GYRO+0] = *(gyro_data+0);       // X axis
-        MSD[GYRO+1] = *(gyro_data+1);       // X axis
-        MSD[GYRO+2] = *(gyro_data+2);       // Y axis
-        MSD[GYRO+3] = *(gyro_data+3);       // Y axis
-        MSD[GYRO+4] = *(gyro_data+4);       // Z axis
-        MSD[GYRO+5] = *(gyro_data+5);       // Z axis
-        
-        
-        // Read acc
-        readMyAcc(acc_data);
-        MSD[ACC+0] = *(acc_data+0);         // X axis
-        MSD[ACC+1] = *(acc_data+1);         // X axis
-        MSD[ACC+2] = *(acc_data+2);         // Y axis
-        MSD[ACC+3] = *(acc_data+3);         // Y axis
-        MSD[ACC+4] = *(acc_data+4);         // Z axis
-        MSD[ACC+5] = *(acc_data+5);         // Z axis
-        
-        // Read magneto
-        readMagneto(magneto_data);
-        //magneto_data[0] = 0x11;
-        //magneto_data[1] = 0x23;
-        MSD[MAGNETO+0] = *(magneto_data+0); // X axis
-        MSD[MAGNETO+1] = *(magneto_data+1); // X axis
-        MSD[MAGNETO+2] = *(magneto_data+2); // Y axis
-        MSD[MAGNETO+3] = *(magneto_data+3); // Y axis
-        MSD[MAGNETO+4] = *(magneto_data+4); // Z axis
-        MSD[MAGNETO+5] = *(magneto_data+5); // Z axis
-        
+    if(adv_type < 1){
+        advPacket.type = 0x00;
+        readGyr((vector3_s *)advPacket.gyr);
+        readAcc((vector3_s *)advPacket.acc);
+        readMag((vector3_s *)advPacket.mag);
     }
     else{
-        // Set adv type
-        MSD[ADV_TYPE] = 0x01;
-        // Read temperature
-        getTemperature(temperature_data);
-        MSD[TEMPERATURE+0] = *(temperature_data+0);
-        MSD[TEMPERATURE+1] = *(temperature_data+1);
-        MSD[TEMPERATURE+2] = *(temperature_data+2);
-        MSD[TEMPERATURE+3] = *(temperature_data+3);
-        
-        // Read light
-        getLight(light_data);
-        MSD[LIGHT+0] = *(light_data+0);
-        MSD[LIGHT+1] = *(light_data+1);
-        MSD[LIGHT+2] = *(light_data+2);
-        MSD[LIGHT+3] = *(light_data+3);
-        
-        // Read Pressure
-        get_pressure(pressure_data);
-        MSD[PRESSURE+0] = *(pressure_data+0);
-        MSD[PRESSURE+1] = *(pressure_data+1);
-        MSD[PRESSURE+2] = *(pressure_data+2);
-        MSD[PRESSURE+3] = *(pressure_data+3);
-        
-        // Read Humidity
-        getHumidity(hum_data);
-        MSD[HUMIDITY+0] = *(hum_data+0);
-        MSD[HUMIDITY+1] = *(hum_data+1);
-        MSD[HUMIDITY+2] = *(hum_data+2);
-        MSD[HUMIDITY+3] = *(hum_data+3);
+        advPacket.type = 0x01;
+        advPacket.temperature = getTemperature();
+        advPacket.light       = getLight();
+        advPacket.humidity    = getHumidity();
+        advPacket.pressure    = mpl115a1->getPressure();
     }
-    adv_type++;
-    if(adv_type>2) adv_type = 0;
+    if(++adv_type > 2) adv_type = 0;
     
     adv_data = ble.getAdvertisingData();
-    adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, MSD, 25);
+    adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advPacket, sizeof(advPacket));
     ble.setAdvertisingData(adv_data);
 }
 
 
-int main(void){    
-        
-    SEND("Main started.\r\n");
+int main(){
     power = 1;
-    SEND("Main power regulator turned ON.\r\n");
-    wait_ms(150);
+    wait_ms(WAKEUP_TIME_DELAY_MS);
     temperaturePower = 1;
-    SEND("Temperature power turned ON.\r\n");
     lightPower = 1;
-    SEND("Light power turned ON.\r\n");
-    shdn = 1;   // Wake up the pressure sensor
-    SEND("Pressure sensor woken up.\r\n");
+    shdn = 1; // Wake up the pressure sensor
+    
+    advPacket.header = APPLICATION_ID;
+    
+    ble.init(bleInitComplete);
     
-    /*
-    while(1){
-        led = !led;
-        wait_ms(500);
-    }
-    */
-    
-    I2C i2c(p19, p20);
-    si = new Si7006(&i2c);
-    mems = new LSM9DS1(i2c);
+    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->startAcc();
     mems->startGyro();
@@ -568,30 +242,21 @@
     led = 1;
     
     Ticker ticker;
-    ticker.attach(wakeMeUp, SLEEP_TIME);   // Wake the device up   
-            
-    ble.init(bleInitComplete);
-    ble.gap().setTxPower(TX_POWER);        // Set TX power to TX_POWER
+    ticker.attach(wakeMeUp, SLEEP_TIME); // Wake the device up
     
-    /* SpinWait for initialization to complete. This is necessary because the
-     * BLE object is used in the main loop below. */
-    while (ble.hasInitialized()  == false) { /* spin loop */ }
-    while (true){
-
-    if (SLEEP && GO_TO_SLEEP){
-        ble.gap().stopAdvertising();
-        sleep();
-        ble.waitForEvent();
-    }
-    else{
-        // I'm awake
-        updateData();
-        ble.gap().startAdvertising();
-        wait_ms(WAKE_UP_TIME);
-        SLEEP = true;
+    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;
         }
     }
-    
-        
-    
 }