fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 18:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs: fix throttle prediction (#21444)

Browse Source 
* tecs: fix throttle prediction

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-04-06 20:13:32 +02:00
committed by GitHub
co-authored by Thomas Stastny
parent 460956fd33
commit 8f520ab019
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+4 -4
View File
@@ -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;
}