Plane: Push the TECS to climb in all circumstances

This commit is contained in:
Stavros Korokithakis 2021-03-28 23:29:47 +03:00 committed by Andrew Tridgell
parent 28fbc73fb6
commit 3c19579988
1 changed files with 1 additions and 1 deletions

View File

@ -507,7 +507,7 @@ void Plane::update_alt()
// ensure we do the initial climb in RTL. We add an extra
// 10m in the demanded height to push TECS to climb
// quickly
target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100);
target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
}
SpdHgt_Controller->update_pitch_throttle(target_alt,