File content as of revision 0:12899fa39f88:

/* 
 * 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;
        }
    }
    
        
    
}