From 9b5b6f377968ad7d307cb271637ae17c3846baa1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 23 Mar 2016 11:29:32 +0900 Subject: [PATCH] AP_MotorsHeli: constrain filtered throttle This is required because we have removed the constraint on the throttle input. This also insures that there is no lag caused by the filtered throttle straying far outside the 0 to 1 range --- libraries/AP_Motors/AP_MotorsHeli.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index b5c17b849d..2c357c31c2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -356,6 +356,14 @@ void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo) void AP_MotorsHeli::update_throttle_filter() { _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); + + // constrain filtered throttle + if (_throttle_filter.get() < 0.0f) { + _throttle_filter.reset(0.0f); + } + if (_throttle_filter.get() > 1.0f) { + _throttle_filter.reset(1.0f); + } } // reset_flight_controls - resets all controls and scalars to flight status