Diff: SensorFusion.cpp

Revision:

34:01dec68de3ed

Parent:

31:d65576185bdf

Child:

35:fb6e4601adf3

--- a/SensorFusion.cpp	Tue May 05 09:59:11 2015 +0000
+++ b/SensorFusion.cpp	Wed May 06 07:50:02 2015 +0000
@@ -3,12 +3,15 @@
 #define DEBUG "SensorFusion"
 #include "Logger.h"
 
+#include "Utils.h"
+
 SensorFusion::SensorFusion(I2C &i2c) :
     delegate(&defaultDelegate),
     accel(i2c), gyro(i2c), magneto(i2c),
     q(1, 0, 0, 0), // output quaternion
     deltat(0.010), // sec
-    beta(0.5) // correction gain
+    beta(0.5), // correction gain
+    fused(0, 0, 0)
 {
 }
 
@@ -61,11 +64,41 @@
                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z,
                    magneto_reading.x, magneto_reading.y, magneto_reading.z);
 
-    delegate->sensorTick(q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
+    delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
+}
+
+void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max)
+{
+    magneto.getCalibration(min, max);
 }
 
 void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
 {
+#define FULL_MADGWICK
+#ifdef SIMPLE_COMPLEMENTARY
+
+    // normalise
+    /*float const anorm = sqrt(ax * ax + ay * ay + az * az);
+    ax /= anorm;
+    ay /= anorm;
+    az /= anorm;
+    */
+    float const signAz = (az < 0) - (0 < az);
+    float const aroll = utils::rad2deg(atan2(-ax, sqrt(ay * ay + az * az)));
+    float const apitch = utils::rad2deg(atan2(ay, signAz * sqrt(az * az + ax * ax)));
+
+    // integrate and fuse
+    float const beta = 0.98;
+    fused.x = fmod(beta * (fused.x + (-gx * deltat)) + (1 - beta) * aroll, 360);
+    fused.y = fmod(beta * (fused.y + (-gy * deltat)) + (1 - beta) * apitch, 360);
+    fused.z = fmod(fused.z + (gz * deltat), 360);
+    
+    //fused.x = fmod(fused.x + 180, 360) - 180;
+    //fused.y = fmod(fused.y + 180, 360) - 180;
+    
+#endif
+    
+#ifdef FULL_MADGWICK
     float q1 = q.w, q2 = q.v.x, q3 = q.v.y, q4 = q.v.z;   // short name local variable for readability
     float norm;
     float s1, s2, s3, s4;
@@ -145,4 +178,5 @@
     q.v.x = q2 * norm;
     q.v.y = q3 * norm;
     q.v.z = q4 * norm;
+#endif
 }