diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 8a6138326f..717a33f657 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -580,18 +580,18 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C // assume airspeed and density-independent delta_throttle to sink/climb rate mapping // TODO: include air density for thrust mappings - const float throttle_increase_max_STE_rate = limit.STE_rate_max * (param.throttle_max - param.throttle_trim); - const float throttle_decrease_min_STE_rate = limit.STE_rate_min * (param.throttle_trim - param.throttle_min); + const float throttle_above_trim_per_ste_rate = (param.throttle_max - param.throttle_trim) / limit.STE_rate_max; + const float throttle_below_trim_per_ste_rate = (param.throttle_trim - param.throttle_min) / limit.STE_rate_min; float throttle_predicted = 0.0f; if (ste_rate.setpoint >= FLT_EPSILON) { // throttle is between trim and maximum - throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint / throttle_increase_max_STE_rate; + throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint * throttle_above_trim_per_ste_rate; } else { // throttle is between trim and minimum - throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint / throttle_decrease_min_STE_rate; + throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint * throttle_below_trim_per_ste_rate; }