Plane: cleanup abort of NAV_SCRIPT

use the same enable flag for tricks and auto NAV_SCRIPT_TIME and
ensure we disable if the script stops controlling

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;
     }

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 &&
+    } else if (nav_scripting_active()) {
-               mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) || (nav_scripting.enabled && nav_scripting.current_ms > 0)) {
         // 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);

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;
 

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 &&
+    if (AP_HAL::millis() - nav_scripting.current_ms > 200) {
-            control_mode == &mode_auto &&
+        // set_target_throttle_rate_rpy has not been called from script in last 200ms
-            mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME;
+        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

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;
     }

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;
     }

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 ||