File content as of revision 5:e2e603447679:

#include "mbed.h"
#include "Imu.h"
#include "hts221.h"
#include "lis3mdl.h"
#include "lps25h.h"
#include "lsm6ds0.h"
#include "struct_IMU.h"

    Imu::Imu() {
     
        rawData.is_Raw = false;
        rawData.TEMPERATURE_Value_C = 0.0f;
        rawData.HUMIDITY_Value = 0.0f;
        rawData.PRESSURE_Value = 0.0f;
        rawData.MAG_Value = new AxesRaw_TypeDef();
        rawData.ACC_Value = new AxesRaw_TypeDef();
        rawData.GYR_Value = new AxesRaw_TypeDef();
        
        offsetData.is_Raw = false;
        offsetData.TEMPERATURE_Value_C = 0.0f;
        offsetData.HUMIDITY_Value = 0.0f;
        offsetData.PRESSURE_Value = 0.0f;
        offsetData.MAG_Value = new AxesRaw_TypeDef();
        offsetData.ACC_Value = new AxesRaw_TypeDef();
        offsetData.GYR_Value = new AxesRaw_TypeDef();
    }
    
    void Imu::refresh(int i) {

        if(i == 1)
        {
            //Offset 
            refresh(0);
            rawData.is_Raw = false;
            
            rawData.TEMPERATURE_Value_C -= offsetData.TEMPERATURE_Value_C;
            rawData.HUMIDITY_Value -= offsetData.HUMIDITY_Value;
            rawData.PRESSURE_Value -= offsetData.PRESSURE_Value;
            
            rawData.MAG_Value->AXIS_X -= offsetData.MAG_Value->AXIS_X;
            rawData.MAG_Value->AXIS_Y -= offsetData.MAG_Value->AXIS_Y;
            rawData.MAG_Value->AXIS_Z -= offsetData.MAG_Value->AXIS_Z;
            
            rawData.ACC_Value->AXIS_X -= offsetData.ACC_Value->AXIS_X;
            rawData.ACC_Value->AXIS_Y -= offsetData.ACC_Value->AXIS_Y;
            rawData.ACC_Value->AXIS_Z -= offsetData.ACC_Value->AXIS_Z;
            
            rawData.GYR_Value->AXIS_X -= offsetData.GYR_Value->AXIS_X;
            rawData.GYR_Value->AXIS_Y -= offsetData.GYR_Value->AXIS_Y;
            rawData.GYR_Value->AXIS_Z -= offsetData.GYR_Value->AXIS_Z;
        }
        else
        {
            // Raw 
            rawData.is_Raw = true;
            setRawHUM();
            setRawTEMP();
            setRawPRES();
            setRawMAG();
            setRawACC();
            setRawGYR();
        }
        
        
    }
    
    Data_IMU * Imu::getData() {
        return &rawData;   
    }
    
    void Imu::setRawHUM() {
        hts221.GetHumidity((float *)&rawData.HUMIDITY_Value);
    }
    
    void Imu::setRawTEMP() {
        hts221.GetTemperature((float *) &rawData.TEMPERATURE_Value_C);
    }
    
    void Imu::setRawPRES() {
        lps25h.GetPressure((float *)&rawData.PRESSURE_Value);
    }
    
    void Imu::setRawMAG() {
        lis3mdl.GetAxes(rawData.MAG_Value);
    }
    
    void Imu::setRawACC() {
        lsm6ds0.Acc_GetAxes(rawData.ACC_Value);
    }
    
    void Imu::setRawGYR() {
        lsm6ds0.Gyro_GetAxes(rawData.GYR_Value);
    }
    
    /* get and set offset */
    
    Data_IMU * Imu::getOffset() {
        return &offsetData;   
    }
    
    void Imu::setOffset() {
        
        refresh(0);
        
        offsetData.is_Raw = rawData.is_Raw;
        
        offsetData.TEMPERATURE_Value_C = rawData.TEMPERATURE_Value_C;
        offsetData.HUMIDITY_Value = rawData.HUMIDITY_Value;
        offsetData.PRESSURE_Value = rawData.PRESSURE_Value;
        
        offsetData.MAG_Value->AXIS_X = rawData.MAG_Value->AXIS_X;
        offsetData.MAG_Value->AXIS_Y = rawData.MAG_Value->AXIS_Y;
        offsetData.MAG_Value->AXIS_Z = rawData.MAG_Value->AXIS_Z;
        
        offsetData.ACC_Value->AXIS_X = rawData.ACC_Value->AXIS_X;
        offsetData.ACC_Value->AXIS_Y = rawData.ACC_Value->AXIS_Y;
        offsetData.ACC_Value->AXIS_Z = rawData.ACC_Value->AXIS_Z;
        
        offsetData.GYR_Value->AXIS_X = rawData.GYR_Value->AXIS_X;
        offsetData.GYR_Value->AXIS_Y = rawData.GYR_Value->AXIS_Y;
        offsetData.GYR_Value->AXIS_Z = rawData.GYR_Value->AXIS_Z;
    }