syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

Limiter/Limiter.cpp

Committer:
komaida424
Date:
2015-02-24
Revision:
6:a50e6d3924f1
Parent:
4:4060309b9cc0
Child:
8:1db19b529b22

File content as of revision 6:a50e6d3924f1:

#include "mbed.h"
#include "Limiter.h"

Limiter::Limiter(float LIMIT)
{
    _limit = LIMIT;
    _rate = 0.05;   
    _last= 0;
    skip = false;
}

void Limiter::differential(float DIFF)
{
    _limit = DIFF;
}

void Limiter::rate(float RATE)
{
    _rate = RATE;    
}

float Limiter::calc(float now)
{
    if ( !skip ) { 
        skip = true; 
        return _last = now;
    }
    float differential = ( now - _last );
//    if ( differential < _limit && differential > -_limit ) _last = now;
    if ( differential > _limit ) _last += _limit * _rate;
    else    {
        if ( differential < -_limit ) _last -= _limit * _rate;
        else _last = now;
    }
    return _last;
}
;