Diff: SensorFusion.cpp

Revision:

35:fb6e4601adf3

Parent:

34:01dec68de3ed

Child:

37:63d355f2cf6a

--- a/SensorFusion.cpp	Wed May 06 07:50:02 2015 +0000
+++ b/SensorFusion.cpp	Fri May 08 06:20:18 2015 +0000
@@ -4,6 +4,7 @@
 #include "Logger.h"
 
 #include "Utils.h"
+#define SIXAXIS
 
 SensorFusion::SensorFusion(I2C &i2c) :
     delegate(&defaultDelegate),
@@ -25,11 +26,13 @@
     accel.powerOn();
     accel.start();
     
+    #ifdef NINEAXIS
     magneto.powerOn();
     if (magneto.performSelfTest() == false) {
         return false;
     }
     magneto.start();
+    #endif
     
     // Since everything is synced to gyro interrupt, start it last
     gyro.setDelegate(*this);
@@ -42,11 +45,15 @@
 void SensorFusion::stop()
 {
     gyro.stop();
+    #ifdef NINEAXIS
     magneto.stop();
+    #endif
     accel.stop();
 
     gyro.powerOff();
+    #ifdef NINEAXIS
     magneto.powerOff();
+    #endif
     accel.powerOff();
 }
 
@@ -58,11 +65,16 @@
     Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
     
     Vector3 const accel_reading = accel.read();
+#ifdef NINEAXIS
     Vector3 const magneto_reading = magneto.read();
-
     updateFilter(  accel_reading.x,   accel_reading.y,   accel_reading.z,
                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z,
                    magneto_reading.x, magneto_reading.y, magneto_reading.z);
+#else
+    Vector3 const magneto_reading(0, 0, 0);
+    updateFilter(  accel_reading.x,   accel_reading.y,   accel_reading.z,
+                   gyro_reading.x,    gyro_reading.y,    gyro_reading.z);
+#endif
 
     delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
 }
@@ -72,33 +84,86 @@
     magneto.getCalibration(min, max);
 }
 
+// 6 axis version
+void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz)
+{
+    float q0 = q.w, q1 = q.v.x, q2 = q.v.y, q3 = q.v.z;   // short name local variable for readability
+    
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0 * q0;
+        _2q1 = 2.0 * q1;
+        _2q2 = 2.0 * q2;
+        _2q3 = 2.0 * q3;
+        _4q0 = 4.0 * q0;
+        _4q1 = 4.0 * q1;
+        _4q2 = 4.0 * q2;
+        _8q1 = 8.0 * q1;
+        _8q2 = 8.0 * q2;
+        q0q0 = q0 * q0;
+        q1q1 = q1 * q1;
+        q2q2 = q2 * q2;
+        q3q3 = q3 * q3;
+
+        // Gradient decent algorithm corrective step
+        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+        s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+        s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay;
+        recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * deltat;
+    q1 += qDot2 * deltat;
+    q2 += qDot3 * deltat;
+    q3 += qDot4 * deltat;
+
+    // Normalise quaternion
+    recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+    
+    // return
+    q.w = q0;
+    q.v.x = q1;
+    q.v.y = q2;
+    q.v.z = q3;
+}
+
 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;
@@ -178,5 +243,4 @@
     q.v.x = q2 * norm;
     q.v.y = q3 * norm;
     q.v.z = q4 * norm;
-#endif
 }