Scripting: added VTOL-expo.lua for estimating MOT_THST_EXPO

ArduCopter/Copter.cpp

@@ -381,6 +381,18 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
     return true;
 }
+
+bool Copter::set_thrust(float thrust)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    Quaternion q{0,0,0,0};
+    mode_guided.set_angle(q, Vector3f{}, thrust, true);
+    return true;
+}
 #endif
 
 #if MODE_CIRCLE_ENABLED == ENABLED

ArduCopter/Copter.h

@@ -665,6 +665,7 @@ class Copter : public AP_Vehicle {
     bool set_target_velocity_NED(const Vector3f& vel_ned) override;
     bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
     bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
+    bool set_thrust(float thrust) override;
 #endif
 #if MODE_CIRCLE_ENABLED == ENABLED
     bool get_circle_radius(float &radius_m) override;

ArduPlane/ArduPlane.cpp

@@ -850,6 +850,19 @@ bool Plane::set_land_descent_rate(float descent_rate)
     return false;
 }
 
+// VTOL thrust override from scripting in GUIDED mode
+bool Plane::set_thrust(float thrust)
+{
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.available() && control_mode == &plane.mode_guided) {
+        quadplane.thrust_override.start_ms = AP_HAL::millis();
+        quadplane.thrust_override.thrust = thrust;
+        return true;
+    }
+#endif
+    return false;
+}
+
 #endif // AP_SCRIPTING_ENABLED
 
 // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL

ArduPlane/Plane.h

@@ -297,6 +297,9 @@ class Plane : public AP_Vehicle {
     Mode *control_mode = &mode_initializing;
     Mode *previous_mode = &mode_initializing;
 
+    // were we in VTOL control when we left the previous mode?
+    bool previous_in_vtol;
+
     // time of last mode change
     uint32_t last_mode_change_ms;
 
@@ -951,6 +954,9 @@ class Plane : public AP_Vehicle {
     // 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);
+
+    // override thrust from scripting
+    bool set_thrust(float thrust) override;
 #endif
 
     // commands.cpp

ArduPlane/quadplane.cpp

@@ -199,8 +199,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: GUIDED_MODE
     // @DisplayName: Enable VTOL in GUIDED mode
-    // @Description: This enables use of VTOL in guided mode. When enabled the aircraft will switch to VTOL flight when the guided destination is reached and hover at the destination.
-    // @Values: 0:Disabled,1:Enabled
+    // @Description: This enables use of VTOL in guided mode. When enabled the aircraft will switch to VTOL flight when the guided destination is reached and hhold position while hovering at the destination. If set to 2 then the vehicle will continue hovering if GUIDED mode is entered from a VTOL mode and is hovering. For QRTL and AUTO mode landings it will continue hovering if in the VTOL control part of the landing sequence.
+    // @Values: 0:Disabled,1:Enabled,2:VTOLIfInVTOL
     // @User: Standard
     AP_GROUPINFO("GUIDED_MODE", 40, QuadPlane, guided_mode, 0),
 
@@ -1005,6 +1005,20 @@ void QuadPlane::run_z_controller(void)
         last_pidz_init_ms = now;
     }
     last_pidz_active_ms = now;
+
+#if AP_SCRIPTING_ENABLED
+    if (thrust_override.start_ms != 0) {
+        // require updates every 50ms at least for thrust control
+        if (now - thrust_override.start_ms > 50) {
+            thrust_override.start_ms = 0;
+            pos_control->init_z_controller();
+        } else {
+            attitude_control->set_throttle_out(thrust_override.thrust, true, 0);
+            return;
+        }
+    }
+#endif // AP_SCRIPTING_ENABLED
+
     pos_control->update_z_controller();
 }
 
@@ -3735,7 +3749,11 @@ bool QuadPlane::guided_mode_enabled(void)
         // loiter turns is a fixed wing only operation
         return false;
     }
-    return guided_mode != 0;
+    if (guided_mode == GUIDED_MODE::G_IF_Q_MODE) {
+        // if previously in a Q mode then do VTOL guided
+        return plane.previous_in_vtol;
+    }
+    return guided_mode != GUIDED_MODE::G_OFF;
 }
 
 /*

ArduPlane/quadplane.h

@@ -363,8 +363,14 @@ class QuadPlane
         QRTL_ALWAYS,
     };
 
+    enum class GUIDED_MODE : uint8_t {
+        G_OFF = 0,
+        G_ON  = 1,
+        G_IF_Q_MODE = 2
+    };
+
     // control if a VTOL GUIDED will be used
-    AP_Int8 guided_mode;
+    AP_Enum<GUIDED_MODE> guided_mode;
 
     // control ESC throttle calibration
     AP_Int8 esc_calibration;
@@ -598,6 +604,13 @@ class QuadPlane
     // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
     bool delay_arming;
 
+#if AP_SCRIPTING_ENABLED
+    struct {
+        uint32_t start_ms;
+        float thrust;
+    } thrust_override;
+#endif
+
     /*
       return true if current mission item is a vtol takeoff
      */

ArduPlane/system.cpp

@@ -278,6 +278,13 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
     Mode &old_previous_mode = *previous_mode;
     Mode &old_mode = *control_mode;
 
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.available()) {
+        // remember if we were in a VTOL mode for GUIDED
+        previous_in_vtol = quadplane.in_vtol_mode() || quadplane.in_vtol_takeoff();
+    }
+#endif
+
     // update control_mode assuming success
     // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
     previous_mode = control_mode;