altb_pmic / Lib_Misc

Lib_Misc

Dependents:   IndNav_QK3_T265

Unwrapper_2pi.cpp@0:3312872854c4, 2019-03-04 (annotated)

Committer:
altb
Date:
Mon Mar 04 11:03:51 2019 +0000
Revision:
0:3312872854c4
Child:
13:c2124af33e67
New Lib Folder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:3312872854c4 1 /*
altb 0:3312872854c4 2 */
altb 0:3312872854c4 3
altb 0:3312872854c4 4 #include "Unwrapper_2pi.h"
altb 0:3312872854c4 5 #define pi 3.141592653589793
altb 0:3312872854c4 6 using namespace std;
altb 0:3312872854c4 7
altb 0:3312872854c4 8 Unwrapper_2pi::Unwrapper_2pi(void)
altb 0:3312872854c4 9 {
altb 0:3312872854c4 10 last_value = 0.0;
altb 0:3312872854c4 11 turns = 0;
altb 0:3312872854c4 12 }
altb 0:3312872854c4 13
altb 0:3312872854c4 14 Unwrapper_2pi::~Unwrapper_2pi() {}
altb 0:3312872854c4 15
altb 0:3312872854c4 16 void Unwrapper_2pi::reset(void)
altb 0:3312872854c4 17 {
altb 0:3312872854c4 18 last_value = 0.0;
altb 0:3312872854c4 19 turns = 0;
altb 0:3312872854c4 20 }
altb 0:3312872854c4 21
altb 0:3312872854c4 22 float Unwrapper_2pi::doStep(float in)
altb 0:3312872854c4 23 {
altb 0:3312872854c4 24 float temp = in + 2*pi*(float)turns;
altb 0:3312872854c4 25 if((temp - last_value) > pi){
altb 0:3312872854c4 26 temp -= 2*pi;
altb 0:3312872854c4 27 turns--;
altb 0:3312872854c4 28 }
altb 0:3312872854c4 29 else if((temp - last_value) < -pi){
altb 0:3312872854c4 30 temp += 2*pi;
altb 0:3312872854c4 31 turns++;
altb 0:3312872854c4 32 }
altb 0:3312872854c4 33 last_value = temp;
altb 0:3312872854c4 34 return (temp);
altb 0:3312872854c4 35 }