altb_pmic
Lib_Misc
Diff: Unwrapper_2pi.cpp
- Revision:
- 0:3312872854c4
- Child:
- 13:c2124af33e67
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper_2pi.cpp Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,35 @@
+/*
+*/
+
+#include "Unwrapper_2pi.h"
+#define pi 3.141592653589793
+using namespace std;
+
+Unwrapper_2pi::Unwrapper_2pi(void)
+{
+ last_value = 0.0;
+ turns = 0;
+}
+
+Unwrapper_2pi::~Unwrapper_2pi() {}
+
+void Unwrapper_2pi::reset(void)
+{
+ last_value = 0.0;
+ turns = 0;
+}
+
+float Unwrapper_2pi::doStep(float in)
+{
+ float temp = in + 2*pi*(float)turns;
+ if((temp - last_value) > pi){
+ temp -= 2*pi;
+ turns--;
+ }
+ else if((temp - last_value) < -pi){
+ temp += 2*pi;
+ turns++;
+ }
+ last_value = temp;
+ return (temp);
+}
\ No newline at end of file