Diff: Imu.cpp

Revision:

3:89e327e1217f

Parent:

2:3088586fe88c

--- a/Imu.cpp	Fri Apr 10 16:38:46 2015 +0000
+++ b/Imu.cpp	Fri Apr 10 22:11:12 2015 +0000
@@ -1,40 +1,122 @@
 #include "mbed.h"
-#include "x_cube_mems.h"
 #include "Imu.h"
-#include "x_cube_mems_i2c.h"
+#include "hts221.h"
+#include "lis3mdl.h"
+#include "lps25h.h"
+#include "lsm6ds0.h"
+#include "struct_IMU.h"
 
     Imu::Imu() {
      
-        TEMPERATURE_Value_C = 0.0f;
-        HUMIDITY_Value = 0.0f;
-        PRESSURE_Value = 0.0f;
-        MAG_Value = new AxesRaw_TypeDef();
-        ACC_Value = new AxesRaw_TypeDef();
-        GYR_Value = new AxesRaw_TypeDef();
+        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();
+        }
+        
         
     }
     
-    float Imu::getHUM() {
-        return HUMIDITY_Value; 
+    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);
     }
     
-    float Imu::getTEMP() {
-        return TEMPERATURE_Value_C;
+    void Imu::setRawPRES() {
+        lps25h.GetPressure((float *)&rawData.PRESSURE_Value);
     }
     
-    float Imu::getPRES() {
-        return 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);
     }
     
-    AxesRaw_TypeDef * Imu::getMAG() {
-        
-        return MAG_Value;
+    /* get and set offset */
+    
+    Data_IMU * Imu::getOffset() {
+        return &offsetData;   
     }
     
-    AxesRaw_TypeDef * Imu::getACC() {
-        return ACC_Value;    
-    }
-    
-    AxesRaw_TypeDef * Imu::getGYR() {
-        return GYR_Value;   
+    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;
     }
\ No newline at end of file