Revision 43:6251c0169f4f, committed 2015-05-27

Comitter:

pvaibhav

Date:

Wed May 27 11:45:00 2015 +0000

Parent:

40:8e852115fe55

Child:

44:5483079fa156

Child:

46:fd5a62296b12

Commit message:

9-axis sensor fusion class added, accelerometer switched to 16G range

Changed in this revision

--- a/Accelerometer.cpp	Tue May 26 11:28:37 2015 +0000
+++ b/Accelerometer.cpp	Wed May 27 11:45:00 2015 +0000
@@ -19,6 +19,7 @@
     write_reg(0x14, 0xB6); // reset
     wait_ms(2); // page 11 says only 1.3ms, nothing for startup time, so assuming 2ms
     write_reg(0x11, 0); // set power normal mode
+    write_reg(0x0f, 12); // set range = 16G
     write_reg(0x10, 12); // set bandwidth = 125 Hz (earlier comment was incorrect)
     return read_reg(0x00) == 0xfa; // verify chip ID
 }

--- 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;
 }
-*/
+

--- a/SensorFusion.h	Tue May 26 11:28:37 2015 +0000
+++ b/SensorFusion.h	Wed May 27 11:45:00 2015 +0000
@@ -50,4 +50,17 @@
     void updateFilter(float ax, float ay, float az, float gx, float gy, float gz);
 };
 
+class SensorFusion9 : public SensorFusion6
+{
+public:
+    SensorFusion9(I2C &i2c);
+    virtual void sensorUpdate(Vector3 gyro_degrees);
+    virtual bool start();
+    virtual void stop();
+    
+protected:
+    Magnetometer    magneto;
+    void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
+};
+
 #endif
\ No newline at end of file