From 6eafcdb558c3f8e989f733e1573db2c431e222e4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 May 2021 20:28:58 +1000 Subject: [PATCH] Plane: added Q_OPTION bit for throttle control of landing speed --- ArduPlane/quadplane.cpp | 41 +++++++++++++++++++++++++++++++++++++++-- ArduPlane/quadplane.h | 4 ++++ 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a0dd42f6b0..f9043bf1c6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -346,8 +346,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: OPTIONS // @DisplayName: quadplane options - // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming. - // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes + // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate + // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -1238,6 +1238,24 @@ void QuadPlane::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_o roll_out_cd = 100 * degrees(atanf(cosf(radians(pitch_out_cd*0.01))*tanf(radians(roll_out_cd*0.01)))); } +/* + get pilot throttle in for landing code. Return value on scale of 0 to 1 +*/ +float QuadPlane::get_pilot_land_throttle(void) const +{ + if (plane.rc_failsafe_active()) { + // assume zero throttle if lost RC + return 0; + } + // get scaled throttle input + float throttle_in = plane.channel_throttle->get_control_in(); + + // normalize to [0,1] + throttle_in /= plane.channel_throttle->get_range(); + + return constrain_float(throttle_in, 0, 1); +} + /* control QACRO mode */ @@ -1406,9 +1424,28 @@ bool QuadPlane::is_flying_vtol(void) const */ float QuadPlane::landing_descent_rate_cms(float height_above_ground) const { + const float max_climb_speed = wp_nav->get_default_speed_up(); float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(), height_above_ground, land_final_alt, land_final_alt+6); + + if ((options & OPTION_THR_LANDING_CONTROL) != 0) { + // allow throttle control for landing speed + float thr_in = get_pilot_land_throttle(); + const float dz = 0.1; + const float thresh1 = 0.5+dz; + const float thresh2 = 0.5-dz; + const float scaling = 1.0 / (0.5 - dz); + if (thr_in > thresh1) { + // start climbing + ret = -(thr_in - thresh1)*scaling*max_climb_speed; + } else if (thr_in > thresh2) { + // hold height + ret = 0; + } else { + ret *= (thresh2 - thr_in)*scaling; + } + } return ret; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 06813ea6ac..080eded8b1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -244,6 +244,9 @@ private: // get pilot lean angle void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const; + // get pilot throttle in for landing code. Return value on scale of 0 to 1 + float get_pilot_land_throttle(void) const; + // initialise throttle_wait when entering mode void init_throttle_wait(); @@ -615,6 +618,7 @@ private: OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13), OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14), + OPTION_THR_LANDING_CONTROL=(1<<15), }; AP_Float takeoff_failure_scalar;