diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 09a023795c..0fd56ce33d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -537,7 +537,7 @@ void Plane::update_alt() update_flight_stage(); #if AP_SCRIPTING_ENABLED - if (plane.nav_scripting.enabled) { + if (nav_scripting_active()) { // don't call TECS while we are in a trick return; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 996943d086..ca2d382c01 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -508,8 +508,7 @@ void Plane::stabilize() if (control_mode == &mode_training) { stabilize_training(speed_scaler); #if AP_SCRIPTING_ENABLED - } else if ((control_mode == &mode_auto && - mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) || (nav_scripting.enabled && nav_scripting.current_ms > 0)) { + } else if (nav_scripting_active()) { // scripting is in control of roll and pitch rates and throttle const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler); const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); @@ -519,9 +518,6 @@ void Plane::stabilize() const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); steering_control.rudder = rudder; } - if (AP_HAL::millis() - nav_scripting.current_ms > 50) { //set_target_throttle_rate_rpy has not been called from script in last 50ms - nav_scripting.current_ms = 0; - } #endif } else if (control_mode == &mode_acro) { stabilize_acro(speed_scaler); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9ba17953a3..729b0f65b6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -518,9 +518,7 @@ private: float throttle_pct; uint32_t start_ms; uint32_t current_ms; - bool done; } nav_scripting; - #endif struct { @@ -1127,7 +1125,7 @@ private: #if AP_SCRIPTING_ENABLED // support for NAV_SCRIPT_TIME mission command - bool nav_scripting_active(void) const; + bool nav_scripting_active(void); bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 53bc5b8387..c8df864fa6 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1160,10 +1160,10 @@ float Plane::get_wp_radius() const */ void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd) { - nav_scripting.done = false; + nav_scripting.enabled = true; nav_scripting.id++; nav_scripting.start_ms = AP_HAL::millis(); - nav_scripting.current_ms = 0; + nav_scripting.current_ms = nav_scripting.start_ms; // start with current roll rate, pitch rate and throttle nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target; @@ -1181,18 +1181,25 @@ bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd) const uint32_t now = AP_HAL::millis(); if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) { gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out"); - nav_scripting.done = true; + nav_scripting.enabled = false; } } - return nav_scripting.done; + return !nav_scripting.enabled; } // check if we are in a NAV_SCRIPT_* command -bool Plane::nav_scripting_active(void) const +bool Plane::nav_scripting_active(void) { - return !nav_scripting.done && - control_mode == &mode_auto && - mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME; + if (AP_HAL::millis() - nav_scripting.current_ms > 200) { + // set_target_throttle_rate_rpy has not been called from script in last 200ms + nav_scripting.enabled = false; + nav_scripting.current_ms = 0; + } + if (control_mode == &mode_auto && + mission.get_current_nav_cmd().id != MAV_CMD_NAV_SCRIPT_TIME) { + nav_scripting.enabled = false; + } + return nav_scripting.enabled; } // support for NAV_SCRIPTING mission command @@ -1216,7 +1223,7 @@ bool Plane::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2 void Plane::nav_script_time_done(uint16_t id) { if (id == nav_scripting.id) { - nav_scripting.done = true; + nav_scripting.enabled = false; } } @@ -1228,6 +1235,7 @@ void Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps nav_scripting.yaw_rate_dps = constrain_float(yaw_rate_dps, -g.acro_yaw_rate, g.acro_yaw_rate); nav_scripting.throttle_pct = constrain_float(throttle_pct, aparm.throttle_min, aparm.throttle_max); nav_scripting.current_ms = AP_HAL::millis(); + nav_scripting.enabled = true; } // enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 169c649456..57d3bcb4be 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -29,7 +29,7 @@ void ModeCruise::update() } #if AP_SCRIPTING_ENABLED - if (plane.nav_scripting.enabled) { + if (plane.nav_scripting_active()) { // while a trick is running unlock heading and zero altitude offset locked_heading = false; lock_timer_ms = 0; @@ -53,7 +53,7 @@ void ModeCruise::update() void ModeCruise::navigate() { #if AP_SCRIPTING_ENABLED - if (plane.nav_scripting.enabled) { + if (plane.nav_scripting_active()) { // don't try to navigate while running trick return; } diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 58eb240b81..d7148f2ef4 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -29,7 +29,7 @@ void ModeLoiter::update() } #if AP_SCRIPTING_ENABLED - if (plane.nav_scripting.enabled) { + if (plane.nav_scripting_active()) { // 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); @@ -100,7 +100,7 @@ void ModeLoiter::navigate() } #if AP_SCRIPTING_ENABLED - if (plane.nav_scripting.enabled) { + if (plane.nav_scripting_active()) { // don't try to navigate while running trick return; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 03fc39115c..141b58c451 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -562,7 +562,7 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } #if AP_SCRIPTING_ENABLED - } else if (plane.nav_scripting.current_ms > 0 && nav_scripting.enabled) { + } else if (nav_scripting_active()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); #endif } else if (control_mode == &mode_stabilize ||