Diff: LSM9DS0.cpp

Revision:

16:682ed25a1f5d

Parent:

15:d433a3236219

Child:

17:5b28bc7fd9ba

--- a/LSM9DS0.cpp	Fri Mar 13 08:15:09 2020 +0000
+++ b/LSM9DS0.cpp	Thu Mar 26 07:34:17 2020 +0000
@@ -868,22 +868,22 @@
     float pitchAcc, rollAcc;
  
     /* Integrate the gyro data(deg/s) over time to get angle */
-    pitch += data[4] * dt;  // Angle around the Y-axis
-    roll +=  data[3] * dt;  // Angle around the X-axis
+    roll += data[4] * dt;  // Angle around the Y-axis of IMU
+    pitch +=  data[3] * dt;  // Angle around the X-axis of IMU
     /* 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[2])*180.0f/PI;
-    rollAcc  = (float)atan2f(-data[1], -data[2])*180.0f/PI;//Let Z toggle to avoid pi to -pi transition.
+    rollAcc = (float)atan2f(data[0], -data[2])*180.0f/PI;
+    pitchAcc  = (float)atan2f(-data[1], -data[2])*180.0f/PI;//Let Z toggle to avoid pi to -pi transition.
   
     /* Apply Complementary Filter */
-    pitch = pitch * 0.98f + pitchAcc * 0.02f;
+    roll = roll * 0.98f + rollAcc * 0.02f;
 //    pitch = pitch * 0.9f + pitchAcc * 0.1f;//0.95
 //    pitch = pitch * 0.94f + pitchAcc * 0.06f;//0.95
 //    if(pitch < 0)// = (pitch > 0 )? pitch:pitch;
 //        pitch = -1*pitch;
 //    roll  = roll  * 0.9f + rollAcc  * 0.1f;
 
-    roll  = roll  * 0.94f + rollAcc  * 0.06f;
+    pitch  = pitch * 0.94f + pitchAcc  * 0.06f;
 
 //    roll = (roll > 0 )? roll:180.0f+roll;
 }
\ No newline at end of file