Plane: added support for NAV_SCRIPT_TIME

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 @@ private:
         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 @@ private:
     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 @@ private:
     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