Diff: SensorFusion.cpp

Revision:

41:731e3cfac19b

Parent:

39:1fa9c0e1ffde

Child:

42:160a37bdaa64

--- a/SensorFusion.cpp	Tue May 19 14:18:30 2015 +0000
+++ b/SensorFusion.cpp	Wed May 20 10:13:14 2015 +0000
@@ -16,155 +16,105 @@
 {
 }
 
+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)
 {
     delegate = &d;
 }
 
-bool SensorFusion::start()
-{
+void SensorFusion::startAccelerometer(){
     accel.powerOn();
-    accel.start();
-    
-    #ifdef NINEAXIS
+    accel.start();    
+};
+
+void SensorFusion::startGyrometer(){
+    gyro.powerOn();
+    gyro.start();    
+};
+
+bool SensorFusion::startMagnetometer(){
     magneto.powerOn();
-    if (magneto.performSelfTest() == false) {
+    if (magneto.performSelfTest() == false){
+        //Should it be left powered on
         return false;
     }
-    magneto.start();
-    #endif
+    magneto.start(); 
+    return true;   
+   
+};
     
-    // Since everything is synced to gyro interrupt, start it last
-    gyro.setDelegate(*this);
-    gyro.powerOn();
-    gyro.start();
-    
-    return true;
-}
+void SensorFusion::stopAccelerometer(){
+    accel.stop();
+    accel.powerOff();    
+};
 
-void SensorFusion::stop()
-{
+void SensorFusion::stopGyrometer(){
     gyro.stop();
-    #ifdef NINEAXIS
+    gyro.powerOff();    
+};
+
+void SensorFusion::stopMagnetometer(){
     magneto.stop();
-    #endif
-    accel.stop();
-
-    gyro.powerOff();
-    #ifdef NINEAXIS
-    magneto.powerOff();
-    #endif
-    accel.powerOff();
-}
+    magneto.powerOff();    
+};
 
 static float const deg_to_radian =  0.0174532925f;
 
-void SensorFusion::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
-
-    delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_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)
-{
-    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;
+/* NineAxesSensor*/
 
-    // 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;   
+NineAxesSensor::NineAxesSensor(I2C &i2c) : SensorFusion(i2c){}
 
-        // 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;
+bool NineAxesSensor::start(){
+    startAccelerometer();
+    
+    bool magnetoMeterSelfTestResult = startMagnetometer(); 
+    if( magnetoMeterSelfTestResult == false){
+        return false;    
+    }
+    
+    //Since everything is synced to gyro interrupt, start it last
+    gyro.setDelegate(*this);
+    startGyrometer();
+    return true;
+};
 
-        // 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 NineAxesSensor::stop(){
+    stopAccelerometer();
+    stopMagnetometer();
+    stopGyrometer();
 }
 
-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, q4 = q.v.z;   // short name local variable for readability
+void NineAxesSensor::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;
     float s1, s2, s3, s4;
 
@@ -242,5 +192,140 @@
     q.w = q1 * norm;
     q.v.x = q2 * norm;
     q.v.y = q3 * norm;
-    q.v.z = q4 * 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);
+}
+
+/* SixAxesSensor */
+
+SixAxesSensor::SixAxesSensor(I2C &i2c) : SensorFusion(i2c){}
+
+bool SixAxesSensor::start(){
+    startAccelerometer();
+    startGyrometer();
+    gyro.setDelegate(*this);
+    return true;
+};
+
+void SixAxesSensor::stop(){
+    stopAccelerometer();
+    stopGyrometer();
 }
+
+void SixAxesSensor::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 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);
+}
+
+ bool SensorFusion::start(){
+     return false;    
+ }
+ 
+ void SensorFusion::stop(){
+     
+ }
+ 
+ void SensorFusion::sensorUpdate(Vector3 gyro_degrees){
+     
+ }                 
+
+
+
+
+