Plane: implement manual forward throttle for QACRO, QSTABILIZE and QHOVER

add parameter Q_FWD_THR_MAX for scaling manual throttle
allow forward motor tilt when disarmed
add FWD_THR_CH as an RC option
change Q_FWD_THR_MAX to percent
change RC_OPTION FWD_THR to 209
move assignment of rc_fwd_thr_ch to one_second_loop
move arming check from forward_throttle_pct to set_servos
change Q_FWD_THR_MAX TO Q_FWD_MANTHR_MAX
expand description of Q_FWD_MANTHR_MAX
add pre-arm check for VTOL manual forward throttle
change VTOL fwd throttle input to percent

ArduPlane/RC_Channel.cpp

@@ -92,6 +92,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     case AUX_FUNC::MANUAL:
     case AUX_FUNC::RTL:
     case AUX_FUNC::TAKEOFF:
+    case AUX_FUNC::FWD_THR:
         break;
 
     case AUX_FUNC::Q_ASSIST:
@@ -113,7 +114,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
         // handle in parent class
         RC_Channel::init_aux_function(ch_option, ch_flag);
         break;
-}
+    }
 }
 
 // do_aux_function - implement the function invoked by auxillary switches
@@ -162,7 +163,9 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
 
     case AUX_FUNC::Q_ASSIST:
         do_aux_function_q_assist_state(ch_flag);
-        break;
+
+    case AUX_FUNC::FWD_THR:
+        break; // VTOL forward throttle input label, nothing to do
 
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);

ArduPlane/quadplane.cpp

@@ -493,6 +493,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("ASSIST_DELAY", 19, QuadPlane, assist_delay, 0.5),
     
+    // @Param: FWD_MANTHR_MAX
+    // @DisplayName: VTOL manual forward throttle max percent
+    // @Description: Maximum value for manual forward throttle; used with RC option FWD_THR (209)
+    // @Range: 0 100
+    // @RebootRequired: False
+    AP_GROUPINFO("FWD_MANTHR_MAX", 20, QuadPlane, fwd_thr_max, 0),
+
     AP_GROUPEND
 };
 
@@ -2919,24 +2926,41 @@ void QuadPlane::Log_Write_QControl_Tuning()
   reduces the need for down pitch which reduces load on the vertical
   lift motors.
  */
-int8_t QuadPlane::forward_throttle_pct(void)
+int8_t QuadPlane::forward_throttle_pct()
 {
     /*
-      in non-VTOL modes or modes without a velocity controller. We
+      Unless an RC channel is assigned for manual forward throttle control,
-      don't use it in QHOVER or QSTABILIZE as they are the primary
+      we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary
       recovery modes for a quadplane and need to be as simple as
-      possible. They will drift with the wind
+      possible. They will drift with the wind.
     */
-    if (!in_vtol_mode() ||
+    if (plane.control_mode == &plane.mode_qacro ||
-        !motors->armed() ||
-        vel_forward.gain <= 0 ||
         plane.control_mode == &plane.mode_qstabilize ||
-        plane.control_mode == &plane.mode_qhover ||
+        plane.control_mode == &plane.mode_qhover) {
-        plane.control_mode == &plane.mode_qautotune ||
+
-        motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::GROUND_IDLE) {
+        if (rc_fwd_thr_ch == nullptr) {
+            return 0;
+        } else {
+            // calculate fwd throttle demand from manual input
+            float fwd_thr = rc_fwd_thr_ch->percent_input();
+
+            // set forward throttle to fwd_thr_max * (manual input + mix): range [0,100]
+            fwd_thr *= .01f * constrain_float(fwd_thr_max, 0, 100);
+            return fwd_thr;
+        }
+    }
+
+    /*
+      in qautotune mode or modes without a velocity controller
+    */
+    if (vel_forward.gain <= 0 ||
+        plane.control_mode == &plane.mode_qautotune) {
         return 0;
     }
 
+    /*
+      in modes with a velocity controller
+    */
     float deltat = (AP_HAL::millis() - vel_forward.last_ms) * 0.001f;
     if (deltat > 1 || deltat < 0) {
         vel_forward.integrator = 0;
@@ -2957,10 +2981,10 @@ int8_t QuadPlane::forward_throttle_pct(void)
         vel_forward.integrator = 0;
         return 0;
     }
+    // get component of velocity error in fwd body frame direction
     Vector3f vel_error_body = ahrs.get_rotation_body_to_ned().transposed() * ((desired_velocity_cms*0.01f) - vel_ned);
 
-    // find component of velocity error in fwd body frame direction
+    float fwd_vel_error = vel_error_body.x;
-    float fwd_vel_error = vel_error_body * Vector3f(1,0,0);
 
     // scale forward velocity error by maximum airspeed
     fwd_vel_error /= MAX(plane.aparm.airspeed_max, 5);

ArduPlane/quadplane.h

@@ -101,7 +101,7 @@ public:
     }
 
     // return desired forward throttle percentage
-    int8_t forward_throttle_pct(void);        
+    int8_t forward_throttle_pct();
     float get_weathervane_yaw_rate_cds(void);
 
     // see if we are flying from vtol point of view
@@ -336,6 +336,10 @@ private:
     // manual throttle curve expo strength
     AP_Float throttle_expo;
 
+    // manual forward throttle input
+    AP_Float fwd_thr_max;
+    RC_Channel *rc_fwd_thr_ch;
+
     // QACRO mode max roll/pitch/yaw rates
     AP_Float acro_roll_rate;
     AP_Float acro_pitch_rate;

ArduPlane/radio.cpp

@@ -40,6 +40,9 @@ void Plane::set_control_channels(void)
         SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100);
     }
 
+    // update manual forward throttle channel assignment
+    quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
+
     if (!arming.is_armed() && arming.arming_required() == AP_Arming::Required::YES_MIN_PWM) {
         SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::Limit::TRIM:SRV_Channel::Limit::MIN);
     }

ArduPlane/servos.cpp

@@ -511,9 +511,14 @@ void Plane::set_servos_controlled(void)
         // manual pass through of throttle while in GUIDED
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
     } else if (quadplane.in_vtol_mode()) {
-        // ask quadplane code for forward throttle
+        int16_t fwd_thr = 0;
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 
+        // if armed and not spooled down ask quadplane code for forward throttle
-            constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
+        if (quadplane.motors->armed() &&
+            quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) {
+
+            fwd_thr = constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle);
+        }
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
     }
 }
 

ArduPlane/tiltrotor.cpp

@@ -92,29 +92,48 @@ void QuadPlane::tiltrotor_continuous_update(void)
     tilt.current_throttle = constrain_float(motors_throttle,
                                             tilt.current_throttle-max_change,
                                             tilt.current_throttle+max_change);
-    
+
     /*
       we are in a VTOL mode. We need to work out how much tilt is
-      needed. There are 3 strategies we will use:
+      needed. There are 4 strategies we will use:
 
-      1) in QSTABILIZE or QHOVER the angle will be set to zero. This
+      1) without manual forward throttle control, the angle will be set to zero
+         in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This
          enables these modes to be used as a safe recovery mode.
 
-      2) in fixed wing assisted flight or velocity controlled modes we
+      2) with manual forward throttle control we will set the angle based on
+         the demanded forward throttle via RC input.
+
+      3) 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_VFWD_GAIN being set
+         Q_VFWD_GAIN being set.
 
-      3) if we are in TRANSITION_TIMER mode then we are transitioning
+      4) if we are in TRANSITION_TIMER mode then we are transitioning
          to forward flight and should put the rotors all the way forward
     */
-    if (plane.control_mode == &plane.mode_qstabilize ||
+
-        plane.control_mode == &plane.mode_qhover ||
+    if (plane.control_mode == &plane.mode_qautotune) {
-        plane.control_mode == &plane.mode_qautotune) {
         tiltrotor_slew(0);
         return;
     }
 
+    // if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode
+    if (!assisted_flight &&
+        (plane.control_mode == &plane.mode_qacro ||
+         plane.control_mode == &plane.mode_qstabilize ||
+         plane.control_mode == &plane.mode_qhover)) {
+        if (rc_fwd_thr_ch == nullptr) {
+            // no manual throttle control, set angle to zero
+            tiltrotor_slew(0);
+        } else {
+            // manual control of forward throttle
+            float settilt = .01f * forward_throttle_pct();
+            tiltrotor_slew(settilt);
+        }
+        return;
+    }
+
     if (assisted_flight &&
         transition_state >= TRANSITION_TIMER) {
         // we are transitioning to fixed wing - tilt the motors all