Plane: implement NAV_SCRIPTING for aerobatics in AUTO mode

ArduPilot · Oct 25, 2021 · f0381bc · f0381bc
1 parent 80bcaeb
commit f0381bc
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 0 deletions.
diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
@@ -505,6 +505,13 @@ void Plane::stabilize()
             plane.stabilize_pitch(speed_scaler);
         }
 #endif
+    } else if (control_mode == &mode_auto &&
+               mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPTING) {
+        // 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);
     } else {
         if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) {
             stabilize_stick_mixing_fbw();

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -1126,6 +1126,9 @@ class Plane : public AP_Vehicle {
     float get_throttle_input(bool no_deadzone=false) const;
     float get_adjusted_throttle_input(bool no_deadzone=false) const;
 
+    // support for NAV_SCRIPTING mission command
+    bool nav_scripting_update(bool completed, float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
+
     enum Failsafe_Action {
         Failsafe_Action_None      = 0,
         Failsafe_Action_RTL       = 1,

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -1142,3 +1142,17 @@ bool Plane::verify_nav_scripting(const AP_Mission::Mission_Command& cmd)
     }
     return nav_scripting.done;
 }
+
+// support for NAV_SCRIPTING mission command
+bool Plane::nav_scripting_update(bool completed, float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps)
+{
+    if (control_mode != &mode_auto ||
+        mission.get_current_nav_cmd().id != MAV_CMD_NAV_SCRIPTING) {
+        return false;
+    }
+    nav_scripting.done = completed;
+    nav_scripting.roll_rate_dps = roll_rate_dps;
+    nav_scripting.pitch_rate_dps = pitch_rate_dps;
+    nav_scripting.throttle_pct = throttle_pct;
+    return true;
+}
diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp
@@ -86,6 +86,11 @@ void ModeAuto::update()
         } else {
             plane.calc_throttle();
         }
+    } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPTING) {
+        // 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);
     } else {
         // we are doing normal AUTO flight, the special cases
         // are for takeoff and landing