Navigator: re-introduce min loiter alt, but only apply it for Loiters w/o a specified altitude setpoint

Also applies to Loiters that are started due to the previous mode being over (Takeoff,
VTOL_Takeoff, Mission).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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();
 }
 

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<px4::params::NAV_TRAFF_AVOID>)    _param_nav_traff_avoid,	/**< avoiding other aircraft is enabled */
 		(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu,	/**< avoidance Distance Unmanned*/
 		(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm,	/**< avoidance Distance Manned*/
+		(ParamFloat<px4::params::NAV_MIN_LTR_ALT>)   _param_min_ltr_alt,	/**< minimum altitude in Loiter mode*/
 
 		// non-navigator parameters: Mission (MIS_*)
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,

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);