diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 11a2039f1edb..820e8a97da8b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -139,7 +139,6 @@ class MulticopterPositionControl : public control::SuperBlock control::BlockParamFloat _manual_thr_min; /**< minimal throttle output when flying in manual mode */ control::BlockParamFloat _manual_thr_max; /**< maximal throttle output when flying in manual mode */ - control::BlockParamFloat _manual_land_alt; /**< altitude where landing is likely flying with sticks but in pos mode */ control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ @@ -409,7 +408,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _home_pos{}, _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), - _manual_land_alt(this, "MIS_LTRMIN_ALT", false), _xy_vel_man_expo(this, "XY_MAN_EXPO"), _z_vel_man_expo(this, "Z_MAN_EXPO"), _hold_dz(this, "HOLD_DZ"), @@ -1568,7 +1566,7 @@ void MulticopterPositionControl::control_auto(float dt) } // Handle the landing gear based on the manual landing alt - const bool high_enough_for_landing_gear = (_pos(2) < _manual_land_alt.get() * 2.0f); + const bool high_enough_for_landing_gear = (-_pos(2) + _home_pos.z > 2.0f); // During a mission or in loiter it's safe to retract the landing gear. if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 18052b60d223..2796d544c626 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1131,8 +1131,17 @@ Mission::do_abort_landing() // loiter at the larger of MIS_LTRMIN_ALT above the landing point // or 2 * FW_CLMBOUT_DIFF above the current altitude float alt_landing = get_absolute_altitude_for_item(_mission_item); - float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), - _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get())); + + // ignore _param_loiter_min_alt if smaller then 0 (-1) + float alt_sp; + + if (_param_loiter_min_alt.get() > 0.0f) { + alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), + _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get())); + + } else { + alt_sp = math::max(alt_landing, _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get())); + } _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.altitude_is_relative = false; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index f3fb6a6e7e52..7aa9bad7a468 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -571,9 +571,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite break; case NAV_CMD_LOITER_TO_ALT: + // initially use current altitude, and switch to mission item altitude once in loiter position - sp->alt = math::max(_navigator->get_global_position()->alt, - _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); + if (_param_loiter_min_alt.get() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1) + sp->alt = math::max(_navigator->get_global_position()->alt, + _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); + + } else { + sp->alt = _navigator->get_global_position()->alt; + } // fall through case NAV_CMD_LOITER_TIME_LIMIT: diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 660360130c52..161951402f1b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -61,15 +61,16 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); * Minimum Loiter altitude * * This is the minimum altitude the system will always obey. The intent is to stay out of ground effect. + * set to -1, if there shouldn't be a minimum loiter altitude * * @unit m - * @min 0 + * @min -1 * @max 80 * @decimal 1 * @increment 0.5 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, 1.2f); +PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, -1.0f); /** * Persistent onboard mission storage