From ef95bf088cac2380fbdfb04f3ab88e8e06609bf8 Mon Sep 17 00:00:00 2001 From: Sergey Bokhantsev Date: Wed, 31 Mar 2021 22:31:28 +0300 Subject: [PATCH] AP_Motors: Constrain get_throttle_hover value with allowed range --- libraries/AP_Motors/AP_MotorsHeli.h | 2 +- libraries/AP_Motors/AP_MotorsMulticopter.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 9539d24969..e1377c31e3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -125,7 +125,7 @@ public: // update estimated throttle required to hover void update_throttle_hover(float dt); - float get_throttle_hover() const override { return _collective_hover; } + float get_throttle_hover() const override { return constrain_float(_collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX); } // accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff bool get_takeoff_collective() const { return _heliflags.takeoff_collective; } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 6da792add2..15e69dbc0e 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -50,7 +50,7 @@ public: // update estimated throttle required to hover void update_throttle_hover(float dt); - virtual float get_throttle_hover() const override { return _throttle_hover; } + virtual float get_throttle_hover() const override { return constrain_float(_throttle_hover, AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); } // passes throttle directly to all motors for ESC calibration. // throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()