aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added Q_OPTION bit for throttle control of landing speed

Browse Source
This commit is contained in:
Andrew Tridgell 2021-05-14 20:28:58 +10:00
parent 24375cf9c1
commit 6eafcdb558
2 changed files with 43 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduPlane/quadplane.cpp
View File

@ -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;
}

4
ArduPlane/quadplane.h
View File

@ -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;