Diff: LSM9DS0.cpp

Revision:

11:9f14f399d569

Parent:

10:73660c3d38f6

Child:

12:916e91f73bc0

--- a/LSM9DS0.cpp	Tue Nov 21 05:55:24 2017 +0000
+++ b/LSM9DS0.cpp	Thu Apr 11 11:54:51 2019 +0000
@@ -683,6 +683,8 @@
     // algorithm to calculate g/(ADC tick) based on that 3-bit value:
     aRes = aScale == A_SCALE_16G ? 16.0f / 32768.0f : 
            (((float) aScale + 1.0f) * 2.0f) / 32768.0f;
+           
+//    debug = aRes;
 }
 
 void LSM9DS0::calcmRes()
@@ -860,7 +862,7 @@
 //    }
 }
 
-void LSM9DS0::complementaryFilter(float * data, float dt)
+void LSM9DS0::complementaryFilter(float data[6], float dt)
 {
     
     float pitchAcc, rollAcc;
@@ -871,10 +873,15 @@
     
     /* 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;
+    pitchAcc = (float)atan2f(data[0], -data[2])*180.0f/PI;
+    rollAcc  = (float)atan2f(-data[1], -data[2])*180.0f/PI;//Let Z toggle to avoid pi to -pi transition.
   
     /* Apply Complementary Filter */
-    pitch = pitch * 0.999f + pitchAcc * 0.001f;
-    roll  = roll  * 0.999f + rollAcc  * 0.001f;
+//    pitch = pitch * 0.999f + pitchAcc * 0.001f;
+    pitch = pitch * 0.9f + pitchAcc * 0.1f;//0.95
+//    if(pitch < 0)// = (pitch > 0 )? pitch:pitch;
+//        pitch = -1*pitch;
+    
+    roll  = roll  * 0.95f + rollAcc  * 0.05f;
+//    roll = (roll > 0 )? roll:180.0f+roll;
 }
\ No newline at end of file