syouichi imamori / Mbed OS MulticopterQuadX

Dependencies:   IAP

Limiter/Limiter.cpp@6:a50e6d3924f1, 2015-02-24 (annotated)

Committer:
komaida424
Date:
Tue Feb 24 09:28:29 2015 +0000
Revision:
6:a50e6d3924f1
Parent:
4:4060309b9cc0
Child:
8:1db19b529b22
Release?revision 3.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 4:4060309b9cc0 1 #include "mbed.h"
komaida424 4:4060309b9cc0 2 #include "Limiter.h"
komaida424 4:4060309b9cc0 3
komaida424 4:4060309b9cc0 4 Limiter::Limiter(float LIMIT)
komaida424 4:4060309b9cc0 5 {
komaida424 6:a50e6d3924f1 6 _limit = LIMIT;
komaida424 6:a50e6d3924f1 7 _rate = 0.05;
komaida424 6:a50e6d3924f1 8 _last= 0;
komaida424 4:4060309b9cc0 9 skip = false;
komaida424 4:4060309b9cc0 10 }
komaida424 4:4060309b9cc0 11
komaida424 6:a50e6d3924f1 12 void Limiter::differential(float DIFF)
komaida424 6:a50e6d3924f1 13 {
komaida424 6:a50e6d3924f1 14 _limit = DIFF;
komaida424 6:a50e6d3924f1 15 }
komaida424 6:a50e6d3924f1 16
komaida424 6:a50e6d3924f1 17 void Limiter::rate(float RATE)
komaida424 6:a50e6d3924f1 18 {
komaida424 6:a50e6d3924f1 19 _rate = RATE;
komaida424 6:a50e6d3924f1 20 }
komaida424 6:a50e6d3924f1 21
komaida424 4:4060309b9cc0 22 float Limiter::calc(float now)
komaida424 4:4060309b9cc0 23 {
komaida424 4:4060309b9cc0 24 if ( !skip ) {
komaida424 4:4060309b9cc0 25 skip = true;
komaida424 6:a50e6d3924f1 26 return _last = now;
komaida424 4:4060309b9cc0 27 }
komaida424 6:a50e6d3924f1 28 float differential = ( now - _last );
komaida424 6:a50e6d3924f1 29 // if ( differential < _limit && differential > -_limit ) _last = now;
komaida424 6:a50e6d3924f1 30 if ( differential > _limit ) _last += _limit * _rate;
komaida424 4:4060309b9cc0 31 else {
komaida424 6:a50e6d3924f1 32 if ( differential < -_limit ) _last -= _limit * _rate;
komaida424 6:a50e6d3924f1 33 else _last = now;
komaida424 4:4060309b9cc0 34 }
komaida424 6:a50e6d3924f1 35 return _last;
komaida424 4:4060309b9cc0 36 }
komaida424 4:4060309b9cc0 37 ;