diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1d876fe88a..aee4b1a1cc 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -772,7 +772,17 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; - item->altitude = _navigator->get_global_position()->alt; + + // check if minimum loiter altitude is specified, and enforce it if so + float loiter_altitude_amsl = _navigator->get_global_position()->alt; + + if (_navigator->get_loiter_min_alt() > FLT_EPSILON) { + loiter_altitude_amsl = math::max(loiter_altitude_amsl, + _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); + } + + item->altitude = loiter_altitude_amsl; + item->loiter_radius = _navigator->get_loiter_radius(); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2b6e345dee..dcff5643d5 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -304,6 +304,7 @@ public: void geofence_breach_check(bool &have_geofence_position_data); // Param access + int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); } int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); } float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); } @@ -445,6 +446,7 @@ private: (ParamInt) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */ (ParamFloat) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/ (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ + (ParamFloat) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/ // non-navigator parameters: Mission (MIS_*) (ParamFloat) _param_mis_takeoff_alt, diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index edf2d19f31..6037cb879c 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -161,3 +161,19 @@ PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADU, 10); * @group Mission */ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); + +/** + * Minimum Loiter altitude + * + * This is the minimum altitude above Home the system will always obey in Loiter (Hold) mode if switched into this + * mode without specifying an altitude (e.g. through Loiter switch on RC). + * Doesn't affect Loiters that are part of Missions or that are entered through a reposition setpoint ("Go to"). + * Set to a negative value to disable. + * + * @unit m + * @min -1 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_MIN_LTR_ALT, -1.f);