Plane: unlock cruise/loiter heading while doing a scripted trick

and reset height
This commit is contained in:
Andrew Tridgell 2022-10-25 19:21:14 +11:00
parent e97bb0c318
commit ff5b4f1b13
3 changed files with 37 additions and 0 deletions

View File

@ -536,6 +536,13 @@ void Plane::update_alt()
update_flight_stage();
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
// don't call TECS while we are in a trick
return;
}
#endif
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
float distance_beyond_land_wp = 0;

View File

@ -28,6 +28,15 @@ void ModeCruise::update()
lock_timer_ms = 0;
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
// while a trick is running unlock heading and zero altitude offset
locked_heading = false;
lock_timer_ms = 0;
plane.set_target_altitude_current();
}
#endif
if (!locked_heading) {
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
plane.update_load_factor();
@ -43,6 +52,12 @@ void ModeCruise::update()
*/
void ModeCruise::navigate()
{
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
// don't try to navigate while running trick
return;
}
#endif
if (!locked_heading &&
plane.channel_roll->get_control_in() == 0 &&
plane.rudder_input() == 0 &&

View File

@ -27,6 +27,14 @@ void ModeLoiter::update()
plane.calc_nav_pitch();
plane.calc_throttle();
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
// while a trick is running we reset altitude
plane.set_target_altitude_current();
plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
}
#endif
}
bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
@ -91,6 +99,13 @@ void ModeLoiter::navigate()
plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
// don't try to navigate while running trick
return;
}
#endif
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
}