diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 853aa908d6..37ac140eae 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -53,6 +53,8 @@ * @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. + * @value 5 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1 (except LANDING). + * @value 6 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2 (except LANDING). * * @group VTOL Attitude Control */ diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 27b93b9362..692db98304 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -497,6 +497,26 @@ float VtolType::pusher_assist() return 0.0f; } + 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) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) { + return 0.0f; + } + + 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) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) { + return 0.0f; + } + break; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 355fe534fb..68c5a89cb6 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -100,7 +100,9 @@ enum VtolForwardActuationMode { ENABLE_WITHOUT_LAND, ENABLE_ABOVE_MPC_LAND_ALT1, ENABLE_ABOVE_MPC_LAND_ALT2, - ENABLE_ALL_MODES + ENABLE_ALL_MODES, + ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND, + ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND }; // these are states that can be applied to a selection of multirotor motors.