Diff: main.cpp

Revision:

0:12899fa39f88

Child:

1:326ce5e200fb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 27 12:12:59 2017 +0000
@@ -0,0 +1,597 @@
+/* 
+ * aconno.de
+ * Made by Jurica Resetar
+ * 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"
+
+#define V0 0.47    /* In volts */
+#define TC 0.01    /* In volts */
+#define VCC (3.6)
+
+#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 */
+
+// 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;
+
+#if DEBUG_PRINT
+    #include "nrf52_uart.h"
+    NRF52_UART serial = NRF52_UART(p25,p26,Baud9600);   //Tx, RX BaudRate
+    uint8_t charCounter;
+    char buffer[256] = {0};
+    #define SEND(...) {uint8_t len = sprintf(buffer, __VA_ARGS__); serial.send(buffer, len);}
+#else
+    #define SEND(...);
+#endif
+
+bool SLEEP = true;
+int8_t txPower = 4;
+
+union float2bytes{ 
+    float f; 
+    char b[sizeof(float)]; 
+};
+
+float2bytes temp2byte;
+float2bytes hum2byte;
+float2bytes light2byte;
+float2bytes pressure2byte;
+
+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;
+
+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){
+    BLE::Instance().gap().startAdvertising();
+}
+
+/**
+ *  Function for waking the core up
+ */
+void wakeMeUp(void){
+        SLEEP = 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 */
+}
+
+/**
+ * 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) {
+        /* In case of error, forward the error handling to onBleInitError */
+        onBleInitError(ble, error);
+        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 *)MSD, MSD_SIZE);
+    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 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];    
+}
+
+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];
+}
+
+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 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 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 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];
+    }
+    memsAccInit[0] /= counter;
+    memsAccInit[1] /= counter;
+    memsAccInit[2] /= counter;
+}
+
+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];
+    }
+    memsGyroInit[0] /= counter;
+    memsGyroInit[1] /= counter;
+    memsGyroInit[2] /= counter;
+}
+
+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;
+    }
+    
+    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];
+}
+
+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
+        
+    }
+    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);
+    }
+    adv_type++;
+    if(adv_type>2) adv_type = 0;
+    
+    adv_data = ble.getAdvertisingData();
+    adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, MSD, 25);
+    ble.setAdvertisingData(adv_data);
+}
+
+
+int main(void){    
+        
+    SEND("Main started.\r\n");
+    power = 1;
+    SEND("Main power regulator turned ON.\r\n");
+    wait_ms(150);
+    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");
+    
+    /*
+    while(1){
+        led = !led;
+        wait_ms(500);
+    }
+    */
+    
+    I2C i2c(p19, p20);
+    si = new Si7006(&i2c);
+    mems = new LSM9DS1(i2c);
+    
+    mems->startAcc();
+    mems->startGyro();
+    mems->startMag();
+    
+    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
+    
+    /* 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;
+        }
+    }
+    
+        
+    
+}