Imu.cpp

00001 #include "mbed.h"
00002 #include "Imu.h"
00003 #include "hts221.h"
00004 #include "lis3mdl.h"
00005 #include "lps25h.h"
00006 #include "lsm6ds0.h"
00007 #include "struct_IMU.h"
00008 
00009     Imu::Imu() {
00010      
00011         rawData.is_Raw = false;
00012         rawData.TEMPERATURE_Value_C = 0.0f;
00013         rawData.HUMIDITY_Value = 0.0f;
00014         rawData.PRESSURE_Value = 0.0f;
00015         rawData.MAG_Value = new AxesRaw_TypeDef();
00016         rawData.ACC_Value = new AxesRaw_TypeDef();
00017         rawData.GYR_Value = new AxesRaw_TypeDef();
00018         
00019         offsetData.is_Raw = false;
00020         offsetData.TEMPERATURE_Value_C = 0.0f;
00021         offsetData.HUMIDITY_Value = 0.0f;
00022         offsetData.PRESSURE_Value = 0.0f;
00023         offsetData.MAG_Value = new AxesRaw_TypeDef();
00024         offsetData.ACC_Value = new AxesRaw_TypeDef();
00025         offsetData.GYR_Value = new AxesRaw_TypeDef();
00026     }
00027     
00028     void Imu::refresh(int i) {
00029 
00030         if(i == 1)
00031         {
00032             //Offset 
00033             refresh(0);
00034             rawData.is_Raw = false;
00035             
00036             rawData.TEMPERATURE_Value_C -= offsetData.TEMPERATURE_Value_C;
00037             rawData.HUMIDITY_Value -= offsetData.HUMIDITY_Value;
00038             rawData.PRESSURE_Value -= offsetData.PRESSURE_Value;
00039             
00040             rawData.MAG_Value->AXIS_X -= offsetData.MAG_Value->AXIS_X;
00041             rawData.MAG_Value->AXIS_Y -= offsetData.MAG_Value->AXIS_Y;
00042             rawData.MAG_Value->AXIS_Z -= offsetData.MAG_Value->AXIS_Z;
00043             
00044             rawData.ACC_Value->AXIS_X -= offsetData.ACC_Value->AXIS_X;
00045             rawData.ACC_Value->AXIS_Y -= offsetData.ACC_Value->AXIS_Y;
00046             rawData.ACC_Value->AXIS_Z -= offsetData.ACC_Value->AXIS_Z;
00047             
00048             rawData.GYR_Value->AXIS_X -= offsetData.GYR_Value->AXIS_X;
00049             rawData.GYR_Value->AXIS_Y -= offsetData.GYR_Value->AXIS_Y;
00050             rawData.GYR_Value->AXIS_Z -= offsetData.GYR_Value->AXIS_Z;
00051         }
00052         else
00053         {
00054             // Raw 
00055             rawData.is_Raw = true;
00056             setRawHUM();
00057             setRawTEMP();
00058             setRawPRES();
00059             setRawMAG();
00060             setRawACC();
00061             setRawGYR();
00062         }
00063         
00064         
00065     }
00066     
00067     Data_IMU * Imu::getData() {
00068         return &rawData;   
00069     }
00070     
00071     void Imu::setRawHUM() {
00072         hts221.GetHumidity((float *)&rawData.HUMIDITY_Value);
00073     }
00074     
00075     void Imu::setRawTEMP() {
00076         hts221.GetTemperature((float *) &rawData.TEMPERATURE_Value_C);
00077     }
00078     
00079     void Imu::setRawPRES() {
00080         lps25h.GetPressure((float *)&rawData.PRESSURE_Value);
00081     }
00082     
00083     void Imu::setRawMAG() {
00084         lis3mdl.GetAxes(rawData.MAG_Value);
00085     }
00086     
00087     void Imu::setRawACC() {
00088         lsm6ds0.Acc_GetAxes(rawData.ACC_Value);
00089     }
00090     
00091     void Imu::setRawGYR() {
00092         lsm6ds0.Gyro_GetAxes(rawData.GYR_Value);
00093     }
00094     
00095     /* get and set offset */
00096     
00097     Data_IMU * Imu::getOffset() {
00098         return &offsetData;   
00099     }
00100     
00101     void Imu::setOffset() {
00102         
00103         refresh(0);
00104         
00105         offsetData.is_Raw = rawData.is_Raw;
00106         
00107         offsetData.TEMPERATURE_Value_C = rawData.TEMPERATURE_Value_C;
00108         offsetData.HUMIDITY_Value = rawData.HUMIDITY_Value;
00109         offsetData.PRESSURE_Value = rawData.PRESSURE_Value;
00110         
00111         offsetData.MAG_Value->AXIS_X = rawData.MAG_Value->AXIS_X;
00112         offsetData.MAG_Value->AXIS_Y = rawData.MAG_Value->AXIS_Y;
00113         offsetData.MAG_Value->AXIS_Z = rawData.MAG_Value->AXIS_Z;
00114         
00115         offsetData.ACC_Value->AXIS_X = rawData.ACC_Value->AXIS_X;
00116         offsetData.ACC_Value->AXIS_Y = rawData.ACC_Value->AXIS_Y;
00117         offsetData.ACC_Value->AXIS_Z = rawData.ACC_Value->AXIS_Z;
00118         
00119         offsetData.GYR_Value->AXIS_X = rawData.GYR_Value->AXIS_X;
00120         offsetData.GYR_Value->AXIS_Y = rawData.GYR_Value->AXIS_Y;
00121         offsetData.GYR_Value->AXIS_Z = rawData.GYR_Value->AXIS_Z;
00122     }