diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 1427e5a07112..853aa908d69b 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -41,9 +41,27 @@ /** - * Maximum allowed down-pitch the controller is able to demand. This prevents large, negative - * lift values being created when facing strong winds. The vehicle will use the pusher motor - * to accelerate forward if necessary. + * Enable/disable usage of fixed-wing actuators in hover to generate forward force (instead of pitching down). + * This technique can be used to avoid the plane having to pitch down in order to move forward. + * This prevents large, negative lift values being created when facing strong winds. + * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL). + * Only active if demaded down pitch is above VT_DWN_PITCH_MAX, and uses VT_FWD_THRUST_SC to get from + * demanded down pitch to fixed-wing actuation. + * + * @value 0 Disable FW forward actuation in hover. + * @value 1 Enable FW forward actuation in hover in altitude, position and auto modes (except LANDING). + * @value 2 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1. + * @value 3 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2. + * @value 4 Enable FW forward actuation in hover in altitude, position and auto modes. + * + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FWD_THRUST_EN, 0); + +/** + * Maximum allowed angle the vehicle is allowed to pitch down to generate forward force + * when fixed-wing forward actuation is active (seeVT_FW_TRHUST_EN). + * If demanded down pitch exceeds this limmit, the fixed-wing forward actuators are used instead. * * @min 0.0 * @max 45.0 @@ -52,16 +70,17 @@ PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f); /** - * Fixed wing thrust scale for hover forward flight. + * Fixed-wing actuator thrust scale for hover forward flight. + * + * Scale applied to the demanded down-pitch to get the fixed-wing forward actuation in hover mode. + * Only active if demaded down pitch is above VT_DWN_PITCH_MAX. + * Enabled via VT_FWD_THRUST_EN. * - * Scale applied to fixed wing thrust being used as source for forward acceleration in multirotor mode. - * This technique can be used to avoid the plane having to pitch down a lot in order to move forward. - * Setting this value to 0 (default) will disable this strategy. * @min 0.0 * @max 2.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f); +PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.7f); /** * Back transition MC motor ramp up time diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 2285f5ced6c3..2816ccd0cbd6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -94,6 +94,12 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); _params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU"); + _params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN"); + _params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1"); + _params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2"); + + _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); + _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); /* fetch initial parameter values */ parameters_update(); @@ -301,6 +307,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.vt_mc_on_fmu, &l); _params.vt_mc_on_fmu = l; + param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode); + param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1); + param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2); + // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { _vtol_type->parameters_update(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 481b3566e562..08b1ea7d36aa 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -207,6 +207,9 @@ class VtolAttitudeControl : public ModuleBase, public px4:: param_t dec_to_pitch_i; param_t back_trans_dec_sp; param_t vt_mc_on_fmu; + param_t vt_forward_thrust_enable_mode; + param_t mpc_land_alt1; + param_t mpc_land_alt2; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index f848c0fef986..a8eb5a50e901 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -422,6 +422,43 @@ bool VtolType::is_channel_set(const int channel, const int target) float VtolType::pusher_assist() { + // Altitude above ground is distance sensor altitude if available, otherwise local z-position + float dist_to_ground = -_local_pos->z; + + if (_local_pos->dist_bottom_valid) { + dist_to_ground = _local_pos->dist_bottom; + } + + // disable pusher assist depending on setting of forward_thrust_enable_mode: + switch (_params->vt_forward_thrust_enable_mode) { + 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) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1 + if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2 + if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) { + return 0.0f; + } + + break; + } + // if the thrust scale param is zero or the drone is not in some position or altitude control mode, // then the pusher-for-pitch strategy is disabled and we can return if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled @@ -434,12 +471,6 @@ float VtolType::pusher_assist() return 0.0f; } - // disable pusher assist during landing - if (_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - return 0.0f; - } - const Dcmf R(Quatf(_v_att->q)); const Dcmf R_sp(Quatf(_v_att_sp->q_d)); const Eulerf euler(R); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 0a98cf819534..8db59fbd2c23 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -76,6 +76,9 @@ struct Params { float dec_to_pitch_i; float back_trans_dec_sp; bool vt_mc_on_fmu; + int vt_forward_thrust_enable_mode; + float mpc_land_alt1; + float mpc_land_alt2; }; // Has to match 1:1 msg/vtol_vehicle_status.msg @@ -92,6 +95,14 @@ enum class vtol_type { STANDARD }; +enum VtolForwardActuationMode { + DISABLE = 0, + ENABLE_WITHOUT_LAND, + ENABLE_ABOVE_MPC_LAND_ALT1, + ENABLE_ABOVE_MPC_LAND_ALT2, + ENABLE_ALL_MODES +}; + // these are states that can be applied to a selection of multirotor motors. // e.g. if we need to shut off some motors after transitioning to fixed wing mode // we can individually disable them while others might still need to be enabled to produce thrust.