diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 715f2183fa..eb0d35c0c5 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -227,24 +227,6 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() return throttle_thrust_hover + ((1.0-throttle_thrust_hover)*_throttle_limit); } -// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000) -int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const -{ - // convert to 0.0 to 1.0 ratio - float throttle_ratio = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min)); - - // apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0 - if (_thrust_curve_expo > 0.0f){ - throttle_ratio = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*throttle_ratio))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get()); - } - - // scale to maximum thrust point - throttle_ratio *= _thrust_curve_max; - - // convert back to pwm range, constrain and return - return (int16_t)constrain_float(throttle_ratio*(pwm_max-pwm_min)+pwm_min, pwm_min, (pwm_max-pwm_min)*_thrust_curve_max+pwm_min); -} - // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1 float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const { diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 598f507ca0..0eca58f59e 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -136,8 +136,7 @@ protected: // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max float get_current_limit_max_throttle(); - // apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000) - int16_t apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const; + // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1 float apply_thrust_curve_and_volt_scaling(float thrust) const; // update_lift_max_from_batt_voltage - used for voltage compensation