syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

Limiter/Limiter.cpp@4:4060309b9cc0, 2014-10-14 (annotated)

Committer:
komaida424
Date:
Tue Oct 14 08:15:03 2014 +0000
Revision:
4:4060309b9cc0
Child:
6:a50e6d3924f1
rev.2.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 4:4060309b9cc0 6 limit = LIMIT;
komaida424 4:4060309b9cc0 7 last= 0;
komaida424 4:4060309b9cc0 8 skip = false;
komaida424 4:4060309b9cc0 9 }
komaida424 4:4060309b9cc0 10
komaida424 4:4060309b9cc0 11 float Limiter::calc(float now)
komaida424 4:4060309b9cc0 12 {
komaida424 4:4060309b9cc0 13 if ( !skip ) {
komaida424 4:4060309b9cc0 14 skip = true;
komaida424 4:4060309b9cc0 15 return last = now;
komaida424 4:4060309b9cc0 16 }
komaida424 4:4060309b9cc0 17 float differential = ( now - last );
komaida424 4:4060309b9cc0 18 if ( differential < limit && differential > -limit ) last = now;
komaida424 4:4060309b9cc0 19 if ( differential > limit ) last += limit * 0.05f;
komaida424 4:4060309b9cc0 20 else {
komaida424 4:4060309b9cc0 21 if ( differential < -limit ) last -= limit * 0.05f;
komaida424 4:4060309b9cc0 22 else last = now;
komaida424 4:4060309b9cc0 23 }
komaida424 4:4060309b9cc0 24 return last;
komaida424 4:4060309b9cc0 25 }
komaida424 4:4060309b9cc0 26 ;