Plane: tiltrotors: allow vectored yaw motor tilt when disarmed

ArduPlane/AP_Arming.cpp

@@ -4,6 +4,8 @@
 #include "AP_Arming.h"
 #include "Plane.h"
 
+constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup
+
 const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
     // variables from parent vehicle
     AP_NESTEDGROUPINFO(AP_Arming, 0),
@@ -188,6 +190,9 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
 
     change_arm_state();
 
+    // rising edge of delay_arming oneshot
+    delay_arming = true;
+
     gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed");
 
     return true;
@@ -245,5 +250,12 @@ void AP_Arming_Plane::update_soft_armed()
     hal.util->set_soft_armed(is_armed() &&
                              hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
     AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
+
+    // update delay_arming oneshot
+    if (delay_arming &&
+        (AP_HAL::millis() - hal.util->get_last_armed_change() >= AP_ARMING_DELAY_MS)) {
+
+        delay_arming = false;
+    }
 }
 

ArduPlane/AP_Arming.h

@@ -28,10 +28,15 @@ class AP_Arming_Plane : public AP_Arming
     bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
 
     void update_soft_armed();
+    bool get_delay_arming() { return delay_arming; };
 
 protected:
     bool ins_checks(bool report) override;
 
 private:
     void change_arm_state(void);
+
+    // oneshot with duration AP_ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
+    // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
+    bool delay_arming;
 };

ArduPlane/quadplane.cpp

@@ -329,7 +329,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: quadplane options
     // @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight, and the vehicle will not use the vertical lift motors to climb during the transition. If AllowFWTakeoff bit is not set then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand bit is not set then fixed wing land on quadplanes will instead perform a VTOL land. If respect takeoff frame is not set the vehicle will interpret all takeoff waypoints as an altitude above the current position. When Use QRTL is set it will replace QLAND with QRTL for failsafe actions when in VTOL modes. When AIRMODE is set AirMode is automatically enabled if arming by RC channel.
-    // @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types,4:Use a fixed wing approach for VTOL landings,5:Use QRTL instead of QLAND for failsafe when in VTOL modes,6:Use idle governor in MANUAL,7:QAssist force enabled,8:Tailsitter QAssist motors only,9:enable AirMode if arming by aux switch
+    // @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types,4:Use a fixed wing approach for VTOL landings,5:Use QRTL instead of QLAND for failsafe when in VTOL modes,6:Use idle governor in MANUAL,7:QAssist force enabled,8:Tailsitter QAssist motors only,9:enable AirMode if arming by aux switch,10:Enable motor tilt when disarmed,11:Delay spoolup for 2 seconds after arming
     AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
 
     AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
@@ -2006,6 +2006,19 @@ void QuadPlane::motors_output(bool run_rate_controller)
         attitude_control->rate_controller_run();
     }
 
+    /* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
+       1) for safety (OPTION_DELAY_ARMING)
+       2) to allow motors to return to vertical (OPTION_DISARMED_TILT)
+     */
+    if ((options & OPTION_DISARMED_TILT) || (options & OPTION_DELAY_ARMING)) {
+        if (plane.arming.get_delay_arming()) {
+            // delay motor start after arming
+            motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
+            motors->output();
+            return;
+        }
+    }
+
 #if ADVANCED_FAILSAFE == ENABLED
     if (!hal.util->get_soft_armed() ||
         (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) ||

ArduPlane/quadplane.h

@@ -195,7 +195,7 @@ class QuadPlane
      // air mode state: OFF, ON
     AirMode air_mode;
 
-   // check for quadplane assistance needed
+    // check for quadplane assistance needed
     bool assistance_needed(float aspeed, bool have_airspeed);
 
     // check if it is safe to provide assistance
@@ -564,6 +564,8 @@ class QuadPlane
         OPTION_Q_ASSIST_FORCE_ENABLE=(1<<7),
         OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
         OPTION_AIRMODE=(1<<9),
+        OPTION_DISARMED_TILT=(1<<10),
+        OPTION_DELAY_ARMING=(1<<11),
     };
 
     AP_Float takeoff_failure_scalar;
@@ -573,6 +575,11 @@ class QuadPlane
 
     float last_land_final_agl;
 
+
+    // oneshot with duration ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
+    // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
+    bool delay_arming;
+
     /*
       return true if current mission item is a vtol takeoff
      */

ArduPlane/tiltrotor.cpp

@@ -374,7 +374,24 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
 
     // calculate the basic tilt amount from current_tilt
     float base_output = zero_out + (tilt.current_tilt * (1 - zero_out));
-
+
+    // for testing when disarmed, apply vectored yaw in proportion to rudder stick
+    // Wait TILT_DELAY_MS after disarming to allow props to spin down first.
+    constexpr uint32_t TILT_DELAY_MS = 3000;
+    uint32_t now = AP_HAL::millis();
+    if (!hal.util->get_soft_armed() && (plane.quadplane.options & OPTION_DISARMED_TILT)) {
+        // this test is subject to wrapping at ~49 days, but the consequences are insignificant
+        if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) {
+            float yaw_out = plane.channel_rudder->get_control_in();
+            yaw_out /= plane.channel_rudder->get_range();
+            float yaw_range = zero_out;
+
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,  1000 * (base_output + yaw_out * yaw_range));
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * (base_output - yaw_out * yaw_range));
+        }
+        return;
+    }
+
     float tilt_threshold = (tilt.max_angle_deg/90.0f);
     bool no_yaw = (tilt.current_tilt > tilt_threshold);
     if (no_yaw) {