altb_pmic / Lib_Misc

Lib_Misc

Dependents:   IndNav_QK3_T265

Unwrapper_2pi.cpp@13:c2124af33e67, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:15:24 2019 +0000
Revision:
13:c2124af33e67
Parent:
0:3312872854c4
Child:
14:d5f47a30ef19
Rearranged Library for IndNav3

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 {
altb2 13:c2124af33e67 24 float temp = in + 2.0f*pi*(float)turns;
altb2 13:c2124af33e67 25 if((temp - last_value) > (float)pi){
altb2 13:c2124af33e67 26 temp -= 2.0f*pi;
altb 0:3312872854c4 27 turns--;
altb 0:3312872854c4 28 }
altb2 13:c2124af33e67 29 else if((temp - last_value) < -(float)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 }