From cf8c211c3586b38143487fc9397e3e0a5a98b360 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 2 Mar 2015 20:39:56 +0900 Subject: [PATCH] Motors: fix thrust curve and add constraint --- libraries/AP_Motors/AP_Motors_Class.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index fd56e20626..5419b7cf7d 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -268,7 +268,8 @@ int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t if (_thrust_curve_expo > 0.0f){ temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get()); } - return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min); + temp_out = constrain_float(temp_out*_thrust_curve_max*(pwm_max-pwm_min)+pwm_min, pwm_min, pwm_max); + return (int16_t)temp_out; } // update_lift_max from battery voltage - used for voltage compensation