Diff: LSM9DS0.cpp

Revision:

2:48eb33afd0fa

Parent:

1:2c34ccab5256

Child:

3:bc0db184f092

diff -r 2c34ccab5256 -r 48eb33afd0fa LSM9DS0.cpp
--- a/LSM9DS0.cpp	Sat Feb 27 09:03:06 2016 +0000
+++ b/LSM9DS0.cpp	Thu Apr 21 07:50:57 2016 +0000
@@ -858,4 +858,23 @@
 //    {
 //        dest[i++] = Wire.read(); // Put read results in the Rx buffer
 //    }
+}
+
+void LSM9DS0::complementaryFilter(float * data, float dt)
+{
+    
+    float pitchAcc, rollAcc;
+ 
+    /* Integrate the gyro data(deg/s) over time to get angle */
+    pitch += data[5] * dt;  // Angle around the Z-axis
+    roll +=  data[3] * dt;  // Angle around the X-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI;
+    rollAcc  = (float)atan2f(data[2], -data[1])*180.0f/PI;
+  
+    /* Apply Complementary Filter */
+    pitch = pitch * 0.88 + pitchAcc * 0.12;
+    roll  = roll  * 0.88 + rollAcc  * 0.12;
 }
\ No newline at end of file