refactoring

markhw42 · Jun 16, 2018 · 917f12a · 917f12a
1 parent f7fa4c1
commit 917f12a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 7 deletions.
diff --git a/src/MPU6050_tockn.cpp b/src/MPU6050_tockn.cpp
@@ -124,14 +124,14 @@ void MPU6050::update(){
 	gyroY -= gyroYoffset;
 	gyroZ -= gyroZoffset;
 
-	interval = millis() - preInterval;
+	interval = (millis() - preInterval) * 0.001;
 
-	angleGyroX += gyroX * (interval * 0.001);
-	angleGyroY += gyroY * (interval * 0.001);
-	angleGyroZ += gyroZ * (interval * 0.001);
+	angleGyroX += gyroX * interval;
+	angleGyroY += gyroY * interval;
+	angleGyroZ += gyroZ * interval;
 
-	angleX = (gyroCoef * (angleX : gyroX * (interval * 0.001))) + (accCoef * angleAccX);
-	angleY = (gyroCoef * (angleY + gyroY * (interval * 0.001))) + (accCoef * angleAccY);
+	angleX = (gyroCoef * (angleX + gyroX * interval)) + (accCoef * angleAccX);
+	angleY = (gyroCoef * (angleY + gyroY * interval)) + (accCoef * angleAccY);
 	angleZ = angleGyroZ;
 
 	preInterval = millis();

diff --git a/src/MPU6050_tockn.h b/src/MPU6050_tockn.h
@@ -82,7 +82,8 @@ class MPU6050{
 
 	float angleX, angleY, angleZ;
 
-	long interval, preInterval;
+  float interval;
+	long preInterval;
 
 	float accCoef, gyroCoef;
 };