altb_pmic / Lib_Misc

Lib_Misc

Dependents:   IndNav_QK3_T265

Unwrapper_2pi.cpp

Committer:
altb2
Date:
2019-10-21
Revision:
13:c2124af33e67
Parent:
0:3312872854c4
Child:
14:d5f47a30ef19

File content as of revision 13:c2124af33e67:

/*  
*/

#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.0f*pi*(float)turns;
    if((temp - last_value) > (float)pi){
        temp -= 2.0f*pi;
        turns--;
        }
    else if((temp - last_value) < -(float)pi){
        temp += 2*pi;
        turns++;
        }
    last_value = temp;
    return (temp);
}