Plane: Improve use of forward flight motors and tilting rotors in Q modes

ArduPlane/mode_auto.cpp

@@ -17,6 +17,8 @@ bool ModeAuto::_enter()
     
     if (plane.quadplane.available() && plane.quadplane.enable == 2) {
         plane.auto_state.vtol_mode = true;
+        // always zero forward throttle demand on entry into VTOL modes
+        quadplane.q_fwd_throttle = 0.0f;
     } else {
         plane.auto_state.vtol_mode = false;
     }

ArduPlane/mode_qacro.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQAcro::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
     quadplane.throttle_wait = false;
     quadplane.transition->force_transition_complete();
     attitude_control->relax_attitude_controllers();

ArduPlane/mode_qautotune.cpp

@@ -7,6 +7,9 @@
 
 bool ModeQAutotune::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
 #if QAUTOTUNE_ENABLED
     return quadplane.qautotune.init();
 #else

ArduPlane/mode_qhover.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQHover::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
     pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);

ArduPlane/mode_qland.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQLand::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
     plane.mode_qloiter._enter();
     quadplane.throttle_wait = false;
     quadplane.setup_target_position();

ArduPlane/mode_qloiter.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQLoiter::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
     // initialise loiter
     loiter_nav->clear_pilot_desired_acceleration();
     loiter_nav->init_target();
@@ -83,6 +86,8 @@ void ModeQLoiter::run()
     plane.nav_roll_cd = loiter_nav->get_roll();
     plane.nav_pitch_cd = loiter_nav->get_pitch();
 
+    plane.quadplane.assign_tilt_to_fwd_thr();
+
     if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
         pos_control->set_externally_limited_xy();
     }

ArduPlane/mode_qrtl.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQRTL::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    plane.quadplane.q_fwd_throttle = 0.0f;
+
     // treat QRTL as QLAND if we are in guided wait takeoff state, to cope
     // with failsafes during GUIDED->AUTO takeoff sequence
     if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
@@ -103,6 +106,9 @@ void ModeQRTL::run()
             // nav roll and pitch are controller by position controller
             plane.nav_roll_cd = pos_control->get_roll_cd();
             plane.nav_pitch_cd = pos_control->get_pitch_cd();
+
+            plane.quadplane.assign_tilt_to_fwd_thr();
+
             if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
                 pos_control->set_externally_limited_xy();
             }

ArduPlane/mode_qstabilize.cpp

@@ -5,6 +5,9 @@
 
 bool ModeQStabilize::_enter()
 {
+    // always zero forward throttle demand on entry into VTOL modes
+    quadplane.q_fwd_throttle = 0.0f;
+
     quadplane.throttle_wait = false;
     return true;
 }
@@ -35,6 +38,8 @@ void ModeQStabilize::update()
         plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max;
         plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
     }
+
+    plane.quadplane.assign_tilt_to_fwd_thr();
 }
 
 // quadplane stabilize mode

ArduPlane/quadplane.cpp

@@ -167,7 +167,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: VFWD_GAIN
     // @DisplayName: Forward velocity hold gain
-    // @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor.
+    // @Description: The use of this parameter is no longer recommended and has been superseded by a method that works in all VTOL  modes with the exception of autotune which is controlled by the Q_FWD_THR_GAIN parameter. This Q_VFD_GAIN parameter controls use of the forward motor in VTOL modes that use the velocity controller. Set to 0 to disable this function. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. 
     // @Range: 0 0.5
     // @Increment: 0.01
     // @User: Standard
@@ -505,6 +505,22 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min, 10),
 
+    // @Param: FWD_THR_GAIN
+    // @DisplayName: Q mode fwd throttle gain
+    // @Description: Gain from forward accel/tilt to forward throttle that is used in all VTOL modes except autotune. Vehicles using separate forward thrust motors, eg quadplanes, should set this parameter to (all up weight) / (maximum combined thrust of forward motors). Vehicles that tilt lifting rotors to provide forward thrust should set this parameter to (all up weight) / (weight lifted by tilting rotors) which for most aircraft can be approximnated as (total number of lifting rotors) / (number of lifting rotors that tilt). When using this method of forward throttle control, the forward tilt angle limit is controlled by the Q_FWD_PIT_LIM parameter and the Q_VFWD_GAIN parameter should be set to 0 to disable the alternative method that works through the velocity controller. Set Q_FWD_THR_GAIN to 0 to disable this function. Do not use this parameter with tail sitters.
+    // @Range: 0.0 5.0
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("FWD_THR_GAIN", 35, QuadPlane, q_fwd_thr_gain, 2.0f),
+
+    // @Param: FWD_PIT_LIM
+    // @DisplayName: Q mode forward pitch limit
+    // @Description: When forward throttle is being controlled by the Q_FWD_THR_GAIN parameter in Q modes, the vehicle forward (nose down) pitch rotation will be limited to the value specified by this parameter and the any additional forward acceleration required will be produced by use of the forward thrust motor(s) or tilting of moveable rotors. Larger values allow the vehicle to pitch more nose down. Set initially to the amount of nose down pitch required to remove wing lift.
+    // @Units: degrees
+    // @Range: 0.0 5.0
+    // @Increment: 10
+    // @User: Standard
+    AP_GROUPINFO("FWD_PIT_LIM", 36, QuadPlane, q_fwd_tilt_lim, 3.0f),
     AP_GROUPEND
 };
 
@@ -638,6 +654,9 @@ bool QuadPlane::setup(void)
         return false;
     }
 
+    q_fwd_throttle = 0.0f;
+    q_fwd_nav_pitch_lim_cd = -aparm.angle_max;
+
     /*
       cope with upgrade from old AP_Motors values for frame_class
      */
@@ -2069,6 +2088,8 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) const
             gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode");
         }
         plane.auto_state.vtol_mode = true;
+        // always zero forward throttle demand on entry into VTOL modes
+        plane.quadplane.q_fwd_throttle = 0.0f;
         return true;
         
     case MAV_VTOL_STATE_FW:
@@ -2539,7 +2560,6 @@ void QuadPlane::vtol_position_controller(void)
             }
         }
 
-        
         if (poscontrol.get_state() == QPOS_APPROACH) {
             poscontrol_init_approach();
         }
@@ -2676,6 +2696,8 @@ void QuadPlane::vtol_position_controller(void)
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();
 
+        assign_tilt_to_fwd_thr();
+
         if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
             pos_control->set_externally_limited_xy();
         }
@@ -2729,6 +2751,8 @@ void QuadPlane::vtol_position_controller(void)
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();
 
+        assign_tilt_to_fwd_thr();
+
         if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
             pos_control->set_externally_limited_xy();
         }
@@ -2773,6 +2797,8 @@ void QuadPlane::vtol_position_controller(void)
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();
 
+        assign_tilt_to_fwd_thr();
+
         // call attitude controller
         set_pilot_yaw_rate_time_constant();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
@@ -2896,6 +2922,39 @@ void QuadPlane::vtol_position_controller(void)
     }
 }
 
+void QuadPlane::assign_tilt_to_fwd_thr(void) {
+#if QAUTOTUNE_ENABLED
+    if (plane.control_mode == &plane.mode_qautotune) {
+        // limiting forward pitch and using forward throttle or rotor tilt is incompatible with auto-tune
+        q_fwd_throttle = 0.0f;
+        q_fwd_nav_pitch_lim_cd = -aparm.angle_max;
+        return;
+    }
+#endif
+    if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_tilt_lim)) {
+        // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift
+        // and are using the forward thrust motor or tilting rotors to provide the forward acceleration
+        float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f));
+        q_fwd_nav_pitch_lim_cd = (int32_t)MIN(-100.0f * q_fwd_tilt_lim, 0.0f);
+        plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, q_fwd_nav_pitch_lim_cd);
+        q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f);
+
+        // To prevent forward motor prop strike, reduce throttle to zero when close to ground
+        // When we are doing horizontal positioning in a VTOL land
+        // we always allow the fwd motor to run. Otherwise a bad
+        // lidar could cause the aircraft not to be able to
+        // approach the landing point when landing below the takeoff point
+        if (!in_vtol_land_approach()) {
+            float alt_cutoff = MAX(0,vel_forward_alt_cutoff);
+            float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+            float fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground, alt_cutoff, alt_cutoff+2);
+            q_fwd_throttle *= fwd_thr_scaler;
+        }
+
+    } else {
+        q_fwd_throttle = 0.0f;
+    }
+}
 
 /*
   we want to limit WP speed to a lower speed when more than 20 degrees
@@ -3021,6 +3080,8 @@ void QuadPlane::takeoff_controller(void)
         // nav roll and pitch are controller by position controller
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();
+
+        assign_tilt_to_fwd_thr();
     }
 
     run_xy_controller();
@@ -3078,6 +3139,8 @@ void QuadPlane::waypoint_controller(void)
     plane.nav_roll_cd = wp_nav->get_roll();
     plane.nav_pitch_cd = wp_nav->get_pitch();
 
+    assign_tilt_to_fwd_thr();
+
     if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
         pos_control->set_externally_limited_xy();
     }
@@ -3536,6 +3599,17 @@ void QuadPlane::Log_Write_QControl_Tuning()
  */
 float QuadPlane::forward_throttle_pct()
 {
+    // handle special case where forward thrust motor is used instead of forward pitch.
+    // but not in autotune as it will upset the results
+#if QAUTOTUNE_ENABLED
+    if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_tilt_lim) && plane.control_mode != &plane.mode_qautotune) {
+#else
+    if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_thr_gain)) {
+#endif
+        // handle special case where we are using forward throttle instead of forward tilt in Q modes
+        return 100.0f * q_fwd_throttle;
+    }
+
     /*
       Unless an RC channel is assigned for manual forward throttle control,
       we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary

ArduPlane/quadplane.h

@@ -395,6 +395,12 @@ private:
     AP_Float acro_pitch_rate;
     AP_Float acro_yaw_rate;
 
+    // gain from forward acceleration to forward throttle
+    AP_Float q_fwd_thr_gain;
+
+    // limit applied to forward pitch to prevent wing producing negative lift
+    AP_Float q_fwd_tilt_lim;
+
     // time we last got an EKF yaw reset
     uint32_t ekfYawReset_ms;
 
@@ -411,6 +417,9 @@ private:
 
     Location last_auto_target;
 
+    float q_fwd_throttle; // forward throttle used in q modes
+    int32_t q_fwd_nav_pitch_lim_cd; // forward tilt limit used in q modes in centi-degrees
+
     // when did we last run the attitude controller?
     uint32_t last_att_control_ms;
 
@@ -683,6 +692,11 @@ private:
      */
     void set_desired_spool_state(AP_Motors::DesiredSpoolState state);
 
+    /*
+      limit forward pitch demand if using rotor tilt or forward flight motor to provide forward acceleration.
+     */
+    void assign_tilt_to_fwd_thr(void);
+
     /*
       get a scaled Q_WP_SPEED based on direction of movement
      */

ArduPlane/tiltrotor.cpp

@@ -258,21 +258,25 @@ void Tiltrotor::continuous_update(void)
 
     /*
       we are in a VTOL mode. We need to work out how much tilt is
-      needed. There are 4 strategies we will use:
+      needed. There are 5 strategies we will use:
 
-      1) without manual forward throttle control, the angle will be set to zero
+      1) With use of a forward throttle controlled by Q_FWD_THR_GAIN in all
-         in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This
+         VTOL modes except Q_AUTOTUNE, we set the angle based on a calculated
-         enables these modes to be used as a safe recovery mode.
+         forward throttle.
 
-      2) with manual forward throttle control we will set the angle based on
+      2) With manual forward throttle control we set the angle based on the
-         the demanded forward throttle via RC input.
+         RC input demanded forward throttle for QACRO, QSTABILIZE and QHOVER.
 
-      3) in fixed wing assisted flight or velocity controlled modes we
+      3) Without a RC input or calculated forward throttle value, the angle
-         will set the angle based on the demanded forward throttle,
+         will be set to zero in QAUTOTUNE, QACRO, QSTABILIZE and QHOVER.
-         with a maximum tilt given by Q_TILT_MAX. This relies on
+         This enables these modes to be used as a safe recovery mode.
-         Q_VFWD_GAIN being set.
 
-      4) if we are in TRANSITION_TIMER mode then we are transitioning
+      4) In fixed wing assisted flight or velocity controlled modes we will
+         set the angle based on the demanded forward throttle, with a maximum
+         tilt given by Q_TILT_MAX. This relies on Q_FWD_THR_GAIN or Q_VFWD_GAIN
+         being set.
+
+      5) if we are in TRANSITION_TIMER mode then we are transitioning
          to forward flight and should put the rotors all the way forward
     */
 
@@ -283,11 +287,23 @@ void Tiltrotor::continuous_update(void)
     }
 #endif
 
-    // if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode
     if (!quadplane.assisted_flight &&
-        (plane.control_mode == &plane.mode_qacro ||
+        is_positive(plane.quadplane.q_fwd_thr_gain) &&
-         plane.control_mode == &plane.mode_qstabilize ||
+        quadplane.is_flying_vtol())
-         plane.control_mode == &plane.mode_qhover)) {
+    {
+        // We are using the rotor tilt functionality controlled by Q_FWD_THR_GAIN which can
+        // operate in all VTOL modes except Q_AUTOTUNE. Forward rotor tilt is used to produce
+        // forward thrust equivalent to what would have been produced by a forward thrust motor
+        // set to quadplane.forward_throttle_pct()
+        const float fwd_g_demand = 0.01f * quadplane.forward_throttle_pct() / plane.quadplane.q_fwd_thr_gain;
+        const float fwd_tilt_deg = MIN(degrees(atanf(fwd_g_demand)), (float)max_angle_deg);
+        slew(MIN(fwd_tilt_deg * (1/90.0), get_forward_flight_tilt()));
+        return;
+    } else if (!quadplane.assisted_flight &&
+               (plane.control_mode == &plane.mode_qacro ||
+               plane.control_mode == &plane.mode_qstabilize ||
+               plane.control_mode == &plane.mode_qhover))
+    {
         if (quadplane.rc_fwd_thr_ch == nullptr) {
             // no manual throttle control, set angle to zero
             slew(0);