Scripting: allow for aerobatics in AUTO mode in plane

ArduPlane/Attitude.cpp

@@ -500,6 +500,15 @@ void Plane::stabilize()
             plane.stabilize_roll(speed_scaler);
             plane.stabilize_pitch(speed_scaler);
         }
+#endif
+#if ENABLE_SCRIPTING
+    } 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);
 #endif
     } else {
         if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) {

ArduPlane/Plane.h

@@ -521,6 +521,18 @@ class Plane : public AP_Vehicle {
         float terrain_correction;
     } auto_state;
 
+#if ENABLE_SCRIPTING
+    // support for scripting nav commands, with verify
+    struct {
+        uint16_t id;
+        float roll_rate_dps;
+        float pitch_rate_dps;
+        float throttle_pct;
+        uint32_t start_ms;
+        bool done;
+    } nav_scripting;
+#endif
+
     struct {
         // roll pitch yaw commanded from external controller in centidegrees
         Vector3l forced_rpy_cd;
@@ -932,6 +944,12 @@ class Plane : public AP_Vehicle {
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     float get_wp_radius() const;
 
+#if ENABLE_SCRIPTING
+    // nav scripting support
+    void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd);
+#endif
+
     // commands.cpp
     void set_guided_WP(void);
     void update_home();
@@ -1119,6 +1137,17 @@ class Plane : public AP_Vehicle {
     float get_throttle_input(bool no_deadzone=false) const;
     float get_adjusted_throttle_input(bool no_deadzone=false) const;
 
+#ifdef ENABLE_SCRIPTING
+    // support for NAV_SCRIPT_TIME mission command
+    bool nav_scripting_active(void) const;
+    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override;
+    void nav_script_time_done(uint16_t id) override;
+
+    // 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;
+#endif
+
     enum Failsafe_Action {
         Failsafe_Action_None      = 0,
         Failsafe_Action_RTL       = 1,

ArduPlane/commands_logic.cpp

@@ -197,6 +197,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
                                             cmd.content.do_engine_control.height_delay_cm*0.01f);
         break;
 
+#if ENABLE_SCRIPTING
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        do_nav_script_time(cmd);
+        break;
+#endif
+
     default:
         // unable to use the command, allow the vehicle to try the next command
         return false;
@@ -292,6 +298,11 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
 
+#if ENABLE_SCRIPTING
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        return verify_nav_script_time(cmd);
+#endif
+
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
@@ -1106,3 +1117,78 @@ float Plane::get_wp_radius() const
 #endif
     return g.waypoint_radius;
 }
+
+#if ENABLE_SCRIPTING
+/*
+  support for scripted navigation, with verify operation for completion
+ */
+void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
+{
+    nav_scripting.done = false;
+    nav_scripting.id++;
+    nav_scripting.start_ms = AP_HAL::millis();
+
+    // start with current roll rate, pitch rate and throttle
+    nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target;
+    nav_scripting.pitch_rate_dps = plane.pitchController.get_pid_info().target;
+    nav_scripting.throttle_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+}
+
+/*
+  wait for scripting to say that the mission item is complete
+ */
+bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd)
+{
+    if (cmd.content.nav_script_time.timeout_s > 0) {
+        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;
+        }
+    }
+    return nav_scripting.done;
+}
+
+// check if we are in a NAV_SCRIPT_* command
+bool Plane::nav_scripting_active(void) const
+{
+    return !nav_scripting.done &&
+            control_mode == &mode_auto &&
+            mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME;
+}
+
+// support for NAV_SCRIPTING mission command
+bool Plane::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
+{
+    if (!nav_scripting_active()) {
+        // not in NAV_SCRIPT_TIME
+        return false;
+    }
+    const auto &c = mission.get_current_nav_cmd().content.nav_script_time;
+    id = nav_scripting.id;
+    cmd = c.command;
+    arg1 = c.arg1;
+    arg2 = c.arg2;
+    return true;
+}
+
+// called when script has completed the command
+void Plane::nav_script_time_done(uint16_t id)
+{
+    if (id == nav_scripting.id) {
+        nav_scripting.done = true;
+    }
+}
+
+// support for NAV_SCRIPTING mission command
+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()) {
+        return false;
+    }
+    nav_scripting.roll_rate_dps = roll_rate_dps;
+    nav_scripting.pitch_rate_dps = pitch_rate_dps;
+    nav_scripting.throttle_pct = throttle_pct;
+    return true;
+}
+#endif // ENABLE_SCRIPTING

ArduPlane/mode_auto.cpp

@@ -86,6 +86,13 @@ void ModeAuto::update()
         } else {
             plane.calc_throttle();
         }
+#if ENABLE_SCRIPTING
+    } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) {
+        // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle
+        plane.nav_roll_cd = plane.ahrs.roll_sensor;
+        plane.nav_pitch_cd = plane.ahrs.pitch_sensor;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
+#endif
     } else {
         // we are doing normal AUTO flight, the special cases
         // are for takeoff and landing

libraries/AP_Mission/AP_Mission.cpp

@@ -406,8 +406,10 @@ bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd)
 /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
 bool AP_Mission::is_nav_cmd(const Mission_Command& cmd)
 {
-    // NAV commands all have ids below MAV_CMD_NAV_LAST except NAV_SET_YAW_SPEED
-    return (cmd.id <= MAV_CMD_NAV_LAST || cmd.id == MAV_CMD_NAV_SET_YAW_SPEED);
+    // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions
+    return (cmd.id <= MAV_CMD_NAV_LAST ||
+            cmd.id == MAV_CMD_NAV_SET_YAW_SPEED ||
+            cmd.id == MAV_CMD_NAV_SCRIPT_TIME);
 }
 
 /// get_next_nav_cmd - gets next "navigation" command found at or after start_index
@@ -1158,6 +1160,13 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         cmd.content.scripting.p3 = packet.param4;
         break;
 
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        cmd.content.nav_script_time.command = packet.param1;
+        cmd.content.nav_script_time.timeout_s = packet.param2;
+        cmd.content.nav_script_time.arg1 = packet.param3;
+        cmd.content.nav_script_time.arg2 = packet.param4;
+        break;
+
     default:
         // unrecognised command
         return MAV_MISSION_UNSUPPORTED;
@@ -1610,6 +1619,13 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         packet.param4 = cmd.content.scripting.p3;
         break;
 
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        packet.param1 = cmd.content.nav_script_time.command;
+        packet.param2 = cmd.content.nav_script_time.timeout_s;
+        packet.param3 = cmd.content.nav_script_time.arg1;
+        packet.param4 = cmd.content.nav_script_time.arg2;
+        break;
+
     default:
         // unrecognised command
         return false;
@@ -2328,6 +2344,8 @@ const char *AP_Mission::Mission_Command::type() const
         return "Jump";
     case MAV_CMD_DO_GO_AROUND:
         return "Go Around";
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        return "NavScriptTime";
 
     default:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_Mission/AP_Mission.h

@@ -221,6 +221,14 @@ class AP_Mission
         float p3;
     };
 
+    // Scripting NAV command (with verify)
+    struct PACKED nav_script_time_Command {
+        uint8_t command;
+        uint8_t timeout_s;
+        float arg1;
+        float arg2;
+    };
+
     union Content {
         // jump structure
         Jump_Command jump;
@@ -291,6 +299,9 @@ class AP_Mission
         // do scripting
         scripting_Command scripting;
 
+        // nav scripting
+        nav_script_time_Command nav_script_time;
+
         // location
         Location location{};      // Waypoint location
     };