IMU acc and gyro fusion

Needs tweaking to get the right amount of acc data

Makefile

@@ -645,6 +645,7 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/control/ImuFusion.o
 OBJECTS += ./src/control/PID.o
 OBJECTS += ./src/math/Utilities.o
 

src/control/ImuFusion.cpp

@@ -0,0 +1,31 @@
+#include "src/control/ImuFusion.h"
+
+namespace control {
+
+ImuFusion::ImuFusion(mpu6000_spi* pImu)
+: m_pImu(pImu)
+, m_angle(0)
+{
+}
+
+float ImuFusion::getAngle(float dT)
+{
+    int axis = 0;
+
+    float rot = m_pImu->read_rot(axis);
+
+    m_angle += dT*rot;
+
+    //float ratio = 0.99; 
+    float ratio = 0.95f;
+    //float ratio = 0.9996;
+
+    float rawAngle = m_pImu->read_acc_deg(axis); // conversion from G to Deg
+
+    m_angle = (m_angle * ratio) + (rawAngle * ((float)1.0f-ratio));
+
+    return m_angle;
+}
+   
+
+}

src/control/ImuFusion.h

@@ -0,0 +1,23 @@
+#include "src/drivers/MPU6000.h"
+
+namespace control {
+
+class ImuFusion
+{
+
+public:
+ 
+    ImuFusion(mpu6000_spi* pImu);
+
+    float getAngle(float dT);
+
+private:
+
+    mpu6000_spi* m_pImu;
+
+    float m_angle;
+
+
+};
+
+}