Navigator: Provide better feedback if no mission present, enforce minimum altitude in loiter and in auto modes

src/modules/navigator/loiter.cpp

@@ -55,7 +55,8 @@
 #include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	MissionBlock(navigator, name)
+	MissionBlock(navigator, name),
+	_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
 {
 	/* load initial params */
 	updateParams();
@@ -74,7 +75,7 @@ void
 Loiter::on_activation()
 {
 	/* set current mission item to loiter */
-	set_loiter_item(&_mission_item);
+	set_loiter_item(&_mission_item, _param_min_alt.get());
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/loiter.h

@@ -59,6 +59,9 @@ public:
 	virtual void on_activation();
 
 	virtual void on_active();
+
+private:
+	control::BlockParamFloat _param_min_alt;
 };
 
 #endif

src/modules/navigator/mission.cpp

@@ -377,6 +377,7 @@ Mission::set_mission_items()
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
+			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
 
@@ -385,6 +386,7 @@ Mission::set_mission_items()
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
+			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 	} else {
@@ -392,21 +394,17 @@ Mission::set_mission_items()
 		if (_mission_type != MISSION_TYPE_NONE) {
 			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			user_feedback_done = true;
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
 
-		} else if (!user_feedback_done) {
-			/* only tell users that we got no mission if there has not been any
-			 * better, more specific feedback yet
-			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
-			 */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
 		}
+
 		_mission_type = MISSION_TYPE_NONE;
 
-		/* set loiter mission item */
+		/* set loiter mission item and ensure that there is a minimum clearance from home */
-		set_loiter_item(&_mission_item);
+		set_loiter_item(&_mission_item, _param_takeoff_alt.get());
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
@@ -418,6 +416,24 @@ Mission::set_mission_items()
 
 		set_mission_finished();
 
+		if (!user_feedback_done) {
+			/* only tell users that we got no mission if there has not been any
+			 * better, more specific feedback yet
+			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
+			 */
+
+			if (_navigator->get_vstatus()->condition_landed) {
+				/* landed, refusing to take off without a mission */
+
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
+			} else {
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
+			}
+
+			user_feedback_done = true;
+
+		}
+
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
 	}

src/modules/navigator/mission_block.cpp

@@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint()
 }
 
 void
-MissionBlock::set_loiter_item(struct mission_item_s *item)
+MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
 	if (_navigator->get_vstatus()->condition_landed) {
 		/* landed, don't takeoff, but switch to IDLE mode */
@@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item)
 			item->altitude = pos_sp_triplet->current.alt;
 
 		} else {
-			/* use current position */
+			/* use current position and use return altitude as clearance */
 			item->lat = _navigator->get_global_position()->lat;
 			item->lon = _navigator->get_global_position()->lon;
 			item->altitude = _navigator->get_global_position()->alt;
+
+			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+				item->altitude = _navigator->get_home_position()->alt + min_clearance;
+			}
 		}
 
 		item->altitude_is_relative = false;

src/modules/navigator/mission_block.h

@@ -91,7 +91,7 @@ protected:
 	/**
 	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
 	 */
-	void set_loiter_item(struct mission_item_s *item);
+	void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
 
 	mission_item_s _mission_item;
 	bool _waypoint_position_reached;