Navigator: Use maximum flight altitude to limit missions

This change limits all mission items to the maximum flight altitude. The mission will still be executed and flown,
but the vehicle will never exceed the mission altitude. This ensures the vehicle can always reach the mission
items. Wether or not the entire mission should be rejected if it falls outside of the fenced area is enforced
in the mission feasibility checker function.

src/modules/land_detector/land_detector_params.c

@@ -206,12 +206,13 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0);
  * hard altitude limit. This setting will
  * be consolidated with the GF_MAX_VER_DIST
  * parameter.
+ * A negative value indicates that there is no limit
  *
  * @unit m
- * @min 1.5
+ * @min -1
  * @max 10000
  * @decimal 2
  * @group Land Detector
  *
  */
-PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, 10000.0f);
+PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -920,12 +920,9 @@ void
 MulticopterPositionControl::limit_altitude()
 {
 	if (_vehicle_land_detected.alt_max > 0.0f) {
-
-		float altitude_above_home = -_pos(2) + _home_pos.z;
-
-		/* in altitude control, lim_pos_sp(2)it setpoint */
-		if (_run_alt_control &&  altitude_above_home > _vehicle_land_detected.alt_max) {
-			_pos_sp(2) = -_vehicle_land_detected.alt_max +  _home_pos.z;
+		/* in altitude control, limit setpoint */
+		if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
+			_pos_sp(2) = -_vehicle_land_detected.alt_max;
 			return;
 		}
 
@@ -939,8 +936,8 @@ MulticopterPositionControl::limit_altitude()
 			float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
 					   _acceleration_z_max_down.get() * delta_t *delta_t;
 
-			if ((-pos_z_next + _home_pos.z) > _vehicle_land_detected.alt_max) {
-				_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
+			if (pos_z_next <= -_vehicle_land_detected.alt_max) {
+				_pos_sp(2) = -_vehicle_land_detected.alt_max;
 				_run_alt_control = true;
 				return;
 			}
@@ -948,7 +945,6 @@ MulticopterPositionControl::limit_altitude()
 	}
 }
 
-
 bool
 MulticopterPositionControl::in_auto_takeoff()
 {

src/modules/navigator/enginefailure.cpp

@@ -126,7 +126,9 @@ EngineFailure::set_ef_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/follow_target.cpp

@@ -361,6 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 
 	pos_sp_triplet->previous.valid = use_position;
 	pos_sp_triplet->previous = pos_sp_triplet->current;
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
 	pos_sp_triplet->current.position_valid = use_position;

src/modules/navigator/land.cpp

@@ -81,12 +81,12 @@ Land::on_activation()
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 	pos_sp_triplet->previous.valid = false;
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
-
 	_navigator->set_can_loiter_at_sp(false);
-
 	_navigator->set_position_setpoint_triplet_updated();
 }
 

src/modules/navigator/loiter.cpp

@@ -129,6 +129,7 @@ Loiter::set_loiter_position()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/mission.cpp

@@ -508,6 +508,7 @@ Mission::set_mission_items()
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
+		mission_apply_limitation(&_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
@@ -764,6 +765,7 @@ Mission::set_mission_items()
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
 
 			/* set position setpoint to target during the transition */
+			mission_apply_limitation(&_mission_item);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 		}
 
@@ -806,6 +808,7 @@ Mission::set_mission_items()
 	/*********************************** set setpoints and check next *********************************************/
 
 	/* set current position setpoint from mission item (is protected against non-position items) */
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	/* issue command if ready (will do nothing for position mission items) */
@@ -832,6 +835,7 @@ Mission::set_mission_items()
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
+			mission_apply_limitation(&_mission_item);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
@@ -1183,6 +1187,7 @@ Mission::do_abort_landing()
 	_mission_item.origin = ORIGIN_ONBOARD;
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	_navigator->set_position_setpoint_triplet_updated();
 

src/modules/navigator/mission_block.cpp

@@ -749,3 +749,24 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
 }
+
+void
+MissionBlock::mission_apply_limitation(struct mission_item_s *item)
+{
+
+	/* do nothing if altitude max is negative */
+	if (_navigator->get_land_detected()->alt_max > 0.0f) {
+
+		/* get absolut altitude */
+		float altitude_abs = item->altitude_is_relative
+				     ? item->altitude + _navigator->get_home_position()->alt
+				     : item->altitude;
+
+		if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
+			item->altitude = item->altitude_is_relative ?
+					 _navigator->get_land_detected()->alt_max :
+					 _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
+
+		}
+	}
+}

src/modules/navigator/mission_block.h

@@ -120,6 +120,12 @@ protected:
 	 */
 	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 
+	/**
+	 * This function limits the setpoint dependent
+	 * on vehicle model
+	*/
+	void	mission_apply_limitation(struct mission_item_s *item);
+
 	void issue_command(const mission_item_s &item);
 
 	float get_time_inside(const struct mission_item_s &item);

src/modules/navigator/navigator.h

@@ -36,6 +36,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Dennis Mannhart <dennis@px4.io>
  */
 
 #ifndef NAVIGATOR_H
@@ -323,11 +324,6 @@ private:
 	 */
 	void		task_main();
 
-	/**
-	 * Translate mission item to a position setpoint.
-	 */
-	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
-
 	/**
 	 * Publish a new position setpoint triplet for position controllers
 	 */

src/modules/navigator/rcloss.cpp

@@ -140,6 +140,7 @@ RCLoss::set_rcl_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/rtl.cpp

@@ -84,6 +84,7 @@ RTL::on_activation()
 {
 	set_current_position_item(&_mission_item);
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
@@ -291,6 +292,7 @@ RTL::set_rtl_item()
 	}
 
 	/* convert mission item to current position setpoint and make it valid */
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/rtl.h

@@ -79,7 +79,6 @@ private:
 	 */
 	float 		get_rtl_altitude();
 
-
 	enum RTLState {
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,

src/modules/navigator/takeoff.cpp

@@ -92,6 +92,7 @@ Takeoff::on_active()
 		// set loiter item so position controllers stop doing takeoff logic
 		set_loiter_item(&_mission_item);
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		mission_apply_limitation(&_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		_navigator->set_position_setpoint_triplet_updated();
 	}
@@ -148,6 +149,7 @@ Takeoff::set_takeoff_position()
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(&_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.yaw_valid = true;