diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 12e793fe1d..4290f8cf1a 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/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); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ef2a52b87b..f72f69adc8 100644 --- a/ArduPlane/quadplane.cpp +++ b/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 - don't use it in QHOVER or QSTABILIZE as they are the primary + 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 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() || - !motors->armed() || - vel_forward.gain <= 0 || + if (plane.control_mode == &plane.mode_qacro || plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover || - plane.control_mode == &plane.mode_qautotune || - motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::GROUND_IDLE) { + plane.control_mode == &plane.mode_qhover) { + + 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 * Vector3f(1,0,0); + float fwd_vel_error = vel_error_body.x; // scale forward velocity error by maximum airspeed fwd_vel_error /= MAX(plane.aparm.airspeed_max, 5); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 3b6f4561e5..816f12714d 100644 --- a/ArduPlane/quadplane.h +++ b/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; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 558b778b83..073099a2c3 100644 --- a/ArduPlane/radio.cpp +++ b/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); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 80f1dacde2..a505831269 100644 --- a/ArduPlane/servos.cpp +++ b/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 - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle)); + int16_t fwd_thr = 0; + // if armed and not spooled down ask quadplane code for forward 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); } } diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 2588a182eb..f2d1ce3a18 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/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 || - plane.control_mode == &plane.mode_qautotune) { + + if (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