| 26 | |
| 27 | |
| 28 | float runningAngle::add(float angle) |
| 29 | { |
| 30 | if (_reset) |
| 31 | { |
| 32 | _average = angle; |
| 33 | _reset = false; |
| 34 | } |
| 35 | else |
| 36 | { |
| 37 | _average = wrap(_average + _weight * wrap(angle - _average)); |
| 38 | } |
| 39 | return _average; |
| 40 | } |
| 41 | |
| 42 | |
| 43 | float runningAngle::wrap(float angle) |