Diff: SensorFusion.cpp

Revision:

42:160a37bdaa64

Parent:

41:731e3cfac19b

--- a/SensorFusion.cpp	Wed May 20 10:13:14 2015 +0000
+++ b/SensorFusion.cpp	Wed May 20 17:20:27 2015 +0000
@@ -2,115 +2,85 @@
 
 #define DEBUG "SensorFusion"
 #include "Logger.h"
-
 #include "Utils.h"
 #define SIXAXIS
-
+    
 SensorFusion::SensorFusion(I2C &i2c) :
+    magnetoMeterEnabled(false),
     delegate(&defaultDelegate),
+    gyroDelegateWithMagneto(this),
+    gyroDelegateWithoutMagneto(this),
     accel(i2c), gyro(i2c), magneto(i2c),
-    q(1, 0, 0, 0), // output quaternion
-    deltat(0.010), // sec
-    beta(0.3), // correction gain
+    q(1, 0, 0, 0),                        // output quaternion
+    deltat(0.010),                        // sec
+    beta(0.3),                            // correction gain
     fused(0, 0, 0)
-{
-}
-
-SensorFusion::SensorFusion(const SensorFusion& s) : 
-    delegate(s.delegate), 
-    accel(s.accel),
-    gyro(s.gyro),
-    magneto(s.magneto),
-    q(s.q),
-    deltat(s.deltat), 
-    beta(s.beta),
-    fused(s.fused) 
 {}
 
-SensorFusion::~SensorFusion(){};
-
-
-SixAxesSensor::~SixAxesSensor(){};
-
-NineAxesSensor::~NineAxesSensor(){};
-
-NineAxesSensor::NineAxesSensor(const NineAxesSensor& c) : SensorFusion(c){};
-
-SixAxesSensor::SixAxesSensor(const SixAxesSensor& c) : SensorFusion(c){};
-
-void SensorFusion::setDelegate(SensorFusion::Delegate &d)
+void SensorFusion::setDelegate(Delegate &d)
 {
     delegate = &d;
 }
 
-void SensorFusion::startAccelerometer(){
-    accel.powerOn();
-    accel.start();    
-};
-
-void SensorFusion::startGyrometer(){
-    gyro.powerOn();
-    gyro.start();    
-};
+void SensorFusion::enableMagnetometer(){
+    magnetoMeterEnabled = true;
+    stop();
+    start();
+}
 
-bool SensorFusion::startMagnetometer(){
-    magneto.powerOn();
-    if (magneto.performSelfTest() == false){
-        //Should it be left powered on
-        return false;
-    }
-    magneto.start(); 
-    return true;   
-   
-};
-    
-void SensorFusion::stopAccelerometer(){
-    accel.stop();
-    accel.powerOff();    
-};
-
-void SensorFusion::stopGyrometer(){
-    gyro.stop();
-    gyro.powerOff();    
-};
-
-void SensorFusion::stopMagnetometer(){
-    magneto.stop();
-    magneto.powerOff();    
-};
-
-static float const deg_to_radian =  0.0174532925f;
-
-void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max)
-{
-    magneto.getCalibration(min, max);
+void SensorFusion::disableMagnetometer(){
+    magnetoMeterEnabled = false;
+    stop();
+    start();
 }
 
-/* NineAxesSensor*/
-
-NineAxesSensor::NineAxesSensor(I2C &i2c) : SensorFusion(i2c){}
-
-bool NineAxesSensor::start(){
-    startAccelerometer();
+bool SensorFusion::start()
+{
+    //Reset quarternion q
+    q.w = 1.0;
+    q.v.x = 0.0;
+    q.v.y = 0.0;
+    q.v.z = 0.0;
     
-    bool magnetoMeterSelfTestResult = startMagnetometer(); 
-    if( magnetoMeterSelfTestResult == false){
-        return false;    
+    accel.powerOn();
+    accel.start();
+ 
+    if(magnetoMeterEnabled){   
+        magneto.powerOn();
+        if (magneto.performSelfTest() == false) {
+            return false;
+        }
+        magneto.start();
+        gyro.setDelegate(gyroDelegateWithMagneto);
+    }else{
+        gyro.setDelegate(gyroDelegateWithoutMagneto);
     }
     
     //Since everything is synced to gyro interrupt, start it last
-    gyro.setDelegate(*this);
-    startGyrometer();
+    gyro.powerOn();
+    gyro.start();
+    
     return true;
-};
-
-void NineAxesSensor::stop(){
-    stopAccelerometer();
-    stopMagnetometer();
-    stopGyrometer();
 }
 
-void NineAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){
+void SensorFusion::stop()
+{
+    gyro.stop();
+    gyro.powerOff();
+        
+    if(magnetoMeterEnabled){
+        magneto.stop();
+        magneto.powerOff();
+    }
+    
+    accel.stop();
+    accel.powerOff();
+}
+
+static float const deg_to_radian =  0.0174532925f;
+            
+void SensorFusion::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, 
@@ -164,11 +134,12 @@
     const float _4bx = 2.0f * _2bx;
     const float _4bz = 2.0f * _2bz;
 
-    // Gradient decent algorithm corrective step
+    // Gradient decent algorithm corrective step    
     s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
     s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
     s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
     s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+    
     norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
     norm = 1.0f/norm;
     s1 *= norm;
@@ -189,41 +160,28 @@
     q4 += qDot4 * deltat;
     norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
     norm = 1.0f/norm;
+    
     q.w = q1 * norm;
     q.v.x = q2 * norm;
     q.v.y = q3 * norm;
-    q.v.z = q4 * norm;    
-}
-
-void NineAxesSensor::sensorUpdate(Vector3 gyro_degrees){
-    Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
-    Vector3 const accel_reading = accel.read();
-    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);
-    
-    delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
+    q.v.z = q4 * norm;
 }
 
-/* SixAxesSensor */
-
-SixAxesSensor::SixAxesSensor(I2C &i2c) : SensorFusion(i2c){}
+SensorFusion::NineAxisSensorFusion::NineAxisSensorFusion(SensorFusion* _ref) : senFuseRef(_ref){}
 
-bool SixAxesSensor::start(){
-    startAccelerometer();
-    startGyrometer();
-    gyro.setDelegate(*this);
-    return true;
-};
-
-void SixAxesSensor::stop(){
-    stopAccelerometer();
-    stopGyrometer();
+void SensorFusion::NineAxisSensorFusion::sensorUpdate(Vector3 gyro_degrees){
+    Vector3 const gyro_reading    = gyro_degrees * deg_to_radian;
+    Vector3 const accel_reading   = senFuseRef->accel.read();
+    Vector3 const magneto_reading = senFuseRef->magneto.read();
+               
+    senFuseRef->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);
+    senFuseRef->delegate->sensorTick(senFuseRef->deltat, senFuseRef->q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, senFuseRef->q);
 }
-
-void SixAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz){
+            
+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, 
@@ -299,33 +257,21 @@
     q.w = q0;
     q.v.x = q1;
     q.v.y = q2;
-    q.v.z = q3;    
-}
-
-void SixAxesSensor::sensorUpdate(Vector3 gyro_degrees){
-    Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
-    Vector3 const accel_reading = accel.read();
-    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);
-                   
-    delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
+    q.v.z = q3;
 }
-
- bool SensorFusion::start(){
-     return false;    
- }
- 
- void SensorFusion::stop(){
-     
- }
- 
- void SensorFusion::sensorUpdate(Vector3 gyro_degrees){
-     
- }                 
-
-
-
-
-
+            
+SensorFusion::SixAxisSensorFusion::SixAxisSensorFusion(SensorFusion* _ref) : senFuseRef(_ref){};
+            
+void SensorFusion::SixAxisSensorFusion::sensorUpdate(Vector3 gyro_degrees){           
+    Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
+    Vector3 const accel_reading = senFuseRef->accel.read();
+    Vector3 const magneto_reading(0, 0, 0);
+                
+    senFuseRef->updateFilter(  accel_reading.x,   accel_reading.y,   accel_reading.z,
+                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z);
+    //parent's delegate
+    senFuseRef->delegate->sensorTick(senFuseRef->deltat, senFuseRef->q.getEulerAngles(), 
+                                    accel_reading, magneto_reading, gyro_degrees, senFuseRef->q);
+}    
+    
+    
\ No newline at end of file