Diff: SensorFusion.cpp

Revision:

40:8e852115fe55

Parent:

39:1fa9c0e1ffde

Child:

43:6251c0169f4f

--- a/SensorFusion.cpp	Tue May 19 14:18:30 2015 +0000
+++ b/SensorFusion.cpp	Tue May 26 11:28:37 2015 +0000
@@ -4,88 +4,62 @@
 #include "Logger.h"
 
 #include "Utils.h"
-#define SIXAXIS
 
-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.3), // correction gain
-    fused(0, 0, 0)
+SensorFusion6::SensorFusion6(I2C &i2c) :
+    SensorFusion(),
+    accel(i2c),
+    gyro(i2c),
+    deltat(0.010), // seconds
+    beta(1),
+    lowpassX(0.93),
+    lowpassY(0.93),
+    lowpassZ(0.93)
 {
+    gyro.setDelegate(*this);
 }
 
-void SensorFusion::setDelegate(SensorFusion::Delegate &d)
+bool SensorFusion6::start()
 {
-    delegate = &d;
-}
-
-bool SensorFusion::start()
-{
+    lowpassX.reset();
+    lowpassY.reset();
+    lowpassZ.reset();
+    
     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);
     gyro.powerOn();
     gyro.start();
     
     return true;
 }
 
-void SensorFusion::stop()
+void SensorFusion6::stop()
 {
     gyro.stop();
-    #ifdef NINEAXIS
-    magneto.stop();
-    #endif
+    gyro.powerOff();
+    
     accel.stop();
-
-    gyro.powerOff();
-    #ifdef NINEAXIS
-    magneto.powerOff();
-    #endif
     accel.powerOff();
 }
 
 static float const deg_to_radian =  0.0174532925f;
 
-void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
-{
-    
+void SensorFusion6::sensorUpdate(Vector3 gyro_degrees)
+{   
     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
+    Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x),
+                                            lowpassY.filter(accel_reading.y),
+                                            lowpassZ.filter(accel_reading.z));
 
-    delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
+    updateFilter( filtered_accel.x,  filtered_accel.y,  filtered_accel.z,
+                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z);
+                    
+    delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, accel_reading, gyro_degrees, q);
 }
 
-void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max)
-{
-    magneto.getCalibration(min, max);
-}
-
-// 6 axis version
-void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz)
+void SensorFusion6::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
     
@@ -162,7 +136,8 @@
     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)
+/*
+void SensorFusion9::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
 {
     float q1 = q.w, q2 = q.v.x, q3 = q.v.y, q4 = q.v.z;   // short name local variable for readability
     float norm;
@@ -244,3 +219,4 @@
     q.v.y = q3 * norm;
     q.v.z = q4 * norm;
 }
+*/