Diff: SensorFusion.cpp

Revision:

43:6251c0169f4f

Parent:

40:8e852115fe55

Child:

44:5483079fa156

Child:

46:fd5a62296b12

--- a/SensorFusion.cpp	Tue May 26 11:28:37 2015 +0000
+++ b/SensorFusion.cpp	Wed May 27 11:45:00 2015 +0000
@@ -10,10 +10,10 @@
     accel(i2c),
     gyro(i2c),
     deltat(0.010), // seconds
-    beta(1),
-    lowpassX(0.93),
-    lowpassY(0.93),
-    lowpassZ(0.93)
+    beta(50),
+    lowpassX(0.96),
+    lowpassY(0.96),
+    lowpassZ(0.96)
 {
     gyro.setDelegate(*this);
 }
@@ -136,7 +136,43 @@
     q.v.z = q3;
 }
 
-/*
+SensorFusion9::SensorFusion9(I2C &i2c) : SensorFusion6(i2c), magneto(i2c)
+{
+    gyro.setDelegate(*this);
+}
+
+bool SensorFusion9::start()
+{
+    magneto.powerOn();
+    magneto.start();
+    
+    return SensorFusion6::start();
+}
+
+void SensorFusion9::stop()
+{
+    SensorFusion6::stop();
+    magneto.stop();
+    magneto.powerOff();
+}
+
+void SensorFusion9::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();
+    
+    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);
+                    
+    delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, magneto_reading, gyro_degrees, q);
+}
+
 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
@@ -219,4 +255,4 @@
     q.v.y = q3 * norm;
     q.v.z = q4 * norm;
 }
-*/
+