Diff: SensorFusion.cpp

Revision:

46:fd5a62296b12

Parent:

43:6251c0169f4f

--- a/SensorFusion.cpp	Wed May 27 11:45:00 2015 +0000
+++ b/SensorFusion.cpp	Wed May 27 13:01:43 2015 +0000
@@ -23,14 +23,14 @@
     lowpassX.reset();
     lowpassY.reset();
     lowpassZ.reset();
-    
+
     accel.powerOn();
     accel.start();
-        
+
     // Since everything is synced to gyro interrupt, start it last
     gyro.powerOn();
     gyro.start();
-    
+
     return true;
 }
 
@@ -38,7 +38,7 @@
 {
     gyro.stop();
     gyro.powerOff();
-    
+
     accel.stop();
     accel.powerOff();
 }
@@ -46,7 +46,7 @@
 static float const deg_to_radian =  0.0174532925f;
 
 void SensorFusion6::sensorUpdate(Vector3 gyro_degrees)
-{   
+{
     Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
     Vector3 const accel_reading = accel.read();
     Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x),
@@ -54,15 +54,15 @@
                                             lowpassZ.filter(accel_reading.z));
 
     updateFilter( filtered_accel.x,  filtered_accel.y,  filtered_accel.z,
-                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z);
-                    
+                  gyro_reading.x,    gyro_reading.y,    gyro_reading.z);
+
     delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, accel_reading, gyro_degrees, q);
 }
 
 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
-    
+
     float recipNorm;
     float s0, s1, s2, s3;
     float qDot1, qDot2, qDot3, qDot4;
@@ -81,7 +81,7 @@
         recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az);
         ax *= recipNorm;
         ay *= recipNorm;
-        az *= recipNorm;   
+        az *= recipNorm;
 
         // Auxiliary variables to avoid repeated arithmetic
         _2q0 = 2.0 * q0;
@@ -128,7 +128,7 @@
     q1 *= recipNorm;
     q2 *= recipNorm;
     q3 *= recipNorm;
-    
+
     // return
     q.w = q0;
     q.v.x = q1;
@@ -145,7 +145,7 @@
 {
     magneto.powerOn();
     magneto.start();
-    
+
     return SensorFusion6::start();
 }
 
@@ -161,15 +161,15 @@
     Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
     Vector3 const accel_reading = accel.read();
     Vector3 const magneto_reading = magneto.read();
-    
+
     Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x),
                                             lowpassY.filter(accel_reading.y),
                                             lowpassZ.filter(accel_reading.z));
 
     updateFilter( filtered_accel.x,  filtered_accel.y,  filtered_accel.z,
-                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z,
-                 magneto_reading.x, magneto_reading.y, magneto_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(), filtered_accel, magneto_reading, gyro_degrees, q);
 }