File content as of revision 4:cb3513aa9814:

/**
 * Made by Jurica @ aconno
 * jurica@aconno.de
 * More info @ aconno.de
 */

#include "mbed.h"
#include "ble/BLE.h"
#include "acd52832_bsp.h"
#include "GapAdvertisingData.h"
#include "Si7006A20.h"
#include "LSM9DS1.h"
#include "math.h"
#include "adc52832_common/utilities.h"
#include "MPL115A1.h"
#include "acd_nrf52_saadc.h"
#include "aconnoConfig.h"
#include <iostream>
#include <cmath>

NRF52_SAADC analogIn;
extern DigitalOut lightPower;
extern DigitalOut temperaturePower;
extern DigitalOut shdn;
extern DigitalOut power;
vector3_s memsAccelerometerInit;
vector3_s memsGyroscopeInit;
vector3_s memsMagnetometerInit;
extern GapAdvertisingData adv_data;
extern advertising_packet advertisementPacket;
DigitalOut cs(p7);

I2C i2c(I2C_DATA, I2C_CLK);   
Si7006 si(&i2c);
SPI spi(SPI_MOSI, SPI_MISO, SPI_SCLK);
MPL115A1 mpl115a1(spi, cs);
LSM9DS1 mems(&i2c);

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

float calculateDegree(float accX, float accY, float accZ){
    
    float degree;
    if (accX != 0.0f && accY != 0.0f && accZ != 0.0f) {

        float dnum = sqrt(accY * accY + accZ * accZ);
        degree = atan(accX / dnum);

        if ((accX < 0 && accY >= 0)) {
            degree = 360 + degree;
        }
        if (accX >= 0 && accY < 0)
            degree = 180 - degree;

        if (accX < 0 && accY < 0)
            degree = 180 - degree;

           //cout<<"degree: " << degree;
           
        }
    return degree;
    
}
void updateData()
{
    //static uint8_t advertisementType = 0;
    int16_t temp_acc[3];
    BLE &ble = BLE::Instance();

    float result;
    mems.startAccelerometer();
   // advertisementPacket.type = 0x00;
    //Added for Angular sensa
     readAccelerometer((vector3_s *)temp_acc);
       float degreeValue = calculateDegree( (float) temp_acc[1],
        (float) temp_acc[0],
         (float) temp_acc[2]);
          
        cout<<"degree: " << degreeValue;  
        advertisementPacket.degree = degreeValue;  
        
    /*if(advertisementType < 1)
    {
        power = 1;
        mems.startAccelerometer();
        mems.startGyroscope();
        mems.startMagnetometer();
        wait_ms(WAKEUP_TIME_DELAY_MS);
        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];
        
        float degreeValue = calculateDegree( (float) temp_acc[1],
        (float) temp_acc[0],
         (float) temp_acc[2]);
          
        cout<<"degree: " << degreeValue;  
        advertisementPacket.degree = degreeValue;  
        // ^--- That's in ug cuz MSB is 1
    }
    else
    {   
        NRF_SAADC->ENABLE = 1;
        power = 1;
        temperaturePower = 1;
        lightPower = 1;
        shdn = 1;
        analogIn.addChannel(9); // Set VDD  as source to SAADC
        analogIn.addChannel(6); // Light
        analogIn.addChannel(7); // Temp
        analogIn.calibrate();
        wait_ms(WAKEUP_TIME_DELAY_MS);
        analogIn.updateData();
        wait_ms(WAKEUP_TIME_DELAY_MS);

        advertisementPacket.type = 0x01;
        advertisementPacket.temperature = getTemperature();
        advertisementPacket.light       = getLight();
        advertisementPacket.humidity    = getHumidity();
        advertisementPacket.pressure    = mpl115a1.getPressure();
        advertisementPacket.battery     = getBattery();
    }
    */
    SPI dummySpi(SPI_MOSI, SPI_MISO, SPI_SCLK);
    power = 0;
    shdn = 0;
    temperaturePower = 0;
    lightPower = 0;
    NRF_SAADC->ENABLE = 0;

    //if(++advertisementType > 2) advertisementType = 0;    
    adv_data = ble.gap().getAdvertisingPayload();
    adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, 
                (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
    ble.gap().setAdvertisingPayload(adv_data);
}