From 5818474a2d98db8908279d356063e6e21763f300 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 6 Mar 2024 08:42:36 +0100 Subject: [PATCH] vtol_type: treat Descend as Land for pusher assist Signed-off-by: Silvan Fuhrer --- .../vtol_att_control/standard_params.c | 1 + src/modules/vtol_att_control/vtol_type.cpp | 23 ++++++++++--------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 51b75a159b06..f46f644f181d 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -48,6 +48,7 @@ * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL). * Only active if demanded down pitch is below VT_PITCH_MIN. * Use VT_FWD_THRUST_SC to tune it. + * Descend mode is treated as Landing too. * * Only active (if enabled) in Altitude, Position and Auto modes, not in Stabilized. * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index aa867102876a..808c9c03c2a9 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -444,16 +444,21 @@ float VtolType::pusher_assist() } + // the vehicle is "landing" if it is in auto mode and the type is set to LAND, and + // "descending" if it is in auto and climb rate controlled but not altitude controlled + const bool vehicle_is_landing_or_descending = _v_control_mode->flag_control_auto_enabled + && ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) || + (_v_control_mode->flag_control_climb_rate_enabled && !_v_control_mode->flag_control_altitude_enabled)); + // disable pusher assist depending on setting of forward_thrust_enable_mode: switch (_param_vt_fwd_thrust_en.get()) { case DISABLE: // disable in all modes return 0.0f; break; - case ENABLE_WITHOUT_LAND: // disable in land mode - if (_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) { + case ENABLE_WITHOUT_LAND: // disable in land/descend mode + if (vehicle_is_landing_or_descending) { return 0.0f; } @@ -473,10 +478,8 @@ float VtolType::pusher_assist() break; - case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land mode - if ((_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) || + case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land/descend mode + if (vehicle_is_landing_or_descending || (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) { return 0.0f; } @@ -484,9 +487,7 @@ float VtolType::pusher_assist() break; case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or in land mode - if ((_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) || + if (vehicle_is_landing_or_descending || (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) { return 0.0f; }