ArduPlane: extend nav_scripting to all modes

ArduPlane/Attitude.cpp

@@ -495,6 +495,22 @@ 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)) {
+        // 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);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
+        if (yawController.rate_control_enabled()) {
+            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);
 #if HAL_QUADPLANE_ENABLED
@@ -509,19 +525,6 @@ void Plane::stabilize()
             plane.stabilize_roll(speed_scaler);
             plane.stabilize_pitch(speed_scaler);
         }
-#endif
-#if AP_SCRIPTING_ENABLED
-    } else if (control_mode == &mode_auto &&
-               mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) {
-        // 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);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
-        if (yawController.rate_control_enabled()) {
-            const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
-            steering_control.rudder = rudder;
-        }
 #endif
     } else {
         if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) {

ArduPlane/Plane.h

@@ -517,14 +517,17 @@ private:
 #if AP_SCRIPTING_ENABLED
     // support for scripting nav commands, with verify
     struct {
+        bool enabled;
         uint16_t id;
         float roll_rate_dps;
         float pitch_rate_dps;
         float yaw_rate_dps;
         float throttle_pct;
         uint32_t start_ms;
+        uint32_t current_ms;
         bool done;
     } nav_scripting;
+    
 #endif
 
     struct {
@@ -1138,8 +1141,9 @@ private:
     // command throttle percentage and roll, pitch, yaw target
     // rates. For use with scripting controllers
     bool set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
+    bool nav_scripting_enable(uint8_t mode) override;
 #endif
-
+ 
     enum Failsafe_Action {
         Failsafe_Action_None      = 0,
         Failsafe_Action_RTL       = 1,

ArduPlane/commands_logic.cpp

@@ -1181,7 +1181,7 @@ void Plane::nav_script_time_done(uint16_t id)
     }
 }
 
-// support for NAV_SCRIPTING mission command
+// support for NAV_SCRIPTING mission command and aerobatics in other allowed modes
 bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps)
 {
     if (!nav_scripting_active()) {
@@ -1191,6 +1191,31 @@ bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps
     nav_scripting.pitch_rate_dps = constrain_float(pitch_rate_dps, -g.acro_pitch_rate, g.acro_pitch_rate);
     nav_scripting.yaw_rate_dps = constrain_float(yaw_rate_dps, -g.acro_yaw_rate, g.acro_yaw_rate);
     nav_scripting.throttle_pct = throttle_pct;
+    nav_scripting.current_ms = AP_HAL::millis();
     return true;
 }
+
+// enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands
+bool Plane::nav_scripting_enable(uint8_t mode)
+{
+   uint8_t current_control_mode = control_mode->mode_number();
+   if (current_control_mode == mode) {
+       switch (current_control_mode) {
+       case Mode::Number::CIRCLE:
+       case Mode::Number::STABILIZE:
+       case Mode::Number::ACRO:
+       case Mode::Number::FLY_BY_WIRE_A:
+       case Mode::Number::FLY_BY_WIRE_B:
+       case Mode::Number::CRUISE:
+       case Mode::Number::LOITER:
+           nav_scripting.enabled = true;
+           break;
+       default:
+           nav_scripting.enabled = false;
+       }
+   } else {
+       nav_scripting.enabled = false;
+   }
+   return nav_scripting.enabled;
+}
 #endif // AP_SCRIPTING_ENABLED

ArduPlane/mode.cpp

@@ -19,6 +19,11 @@ void Mode::exit()
 
 bool Mode::enter()
 {
+#if AP_SCRIPTING_ENABLED
+    // reset nav_scripting.enabled
+    plane.nav_scripting.enabled = false;
+#endif
+
     // cancel inverted flight
     plane.auto_state.inverted_flight = false;
 

ArduPlane/mode_acro.cpp

@@ -5,7 +5,6 @@ bool ModeAcro::_enter()
 {
     plane.acro_state.locked_roll = false;
     plane.acro_state.locked_pitch = false;
-
     return true;
 }
 

ArduPlane/servos.cpp

@@ -551,6 +551,10 @@ void Plane::set_servos_controlled(void)
             // manual pass through of throttle while throttle is suppressed
             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) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
+#endif
     } else if (control_mode == &mode_stabilize ||
                control_mode == &mode_training ||
                control_mode == &mode_acro ||