altb_pmic / Lib_Misc

Lib_Misc

Dependents:   IndNav_QK3_T265

Diff: Unwrapper_2pi.cpp

Revision:
13:c2124af33e67
Parent:
0:3312872854c4
Child:
14:d5f47a30ef19
--- a/Unwrapper_2pi.cpp	Wed Oct 09 13:46:36 2019 +0000
+++ b/Unwrapper_2pi.cpp	Mon Oct 21 17:15:24 2019 +0000
@@ -21,12 +21,12 @@
 
 float Unwrapper_2pi::doStep(float in)
 {
-    float temp = in + 2*pi*(float)turns;
-    if((temp - last_value) > pi){
-        temp -= 2*pi;
+    float temp = in + 2.0f*pi*(float)turns;
+    if((temp - last_value) > (float)pi){
+        temp -= 2.0f*pi;
         turns--;
         }
-    else if((temp - last_value) < -pi){
+    else if((temp - last_value) < -(float)pi){
         temp += 2*pi;
         turns++;
         }