Remove LNDMC_ALT_MAX

This should be replaced with the maximum geofence
altitude GF_MAX_VER_DIST. I'm not aware anyone is using
this functionality as is.

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -103,7 +103,6 @@ void FlightModeManager::Run()
 		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f);
 		_time_stamp_last_loop = time_stamp_now;
 
-		_home_position_sub.update();
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
 		_vehicle_status_sub.update();
@@ -330,9 +329,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		constraints = _current_task.task->getConstraints();
 	}
 
-	// limit altitude according to land detector
-	limitAltitude(setpoint, vehicle_local_position);
-
 	if (_takeoff_status_sub.updated()) {
 		takeoff_status_s takeoff_status;
 
@@ -366,25 +362,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_old_landing_gear_position = landing_gear.landing_gear;
 }
 
-void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
-				      const vehicle_local_position_s &vehicle_local_position)
-{
-	if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
-	    || !vehicle_local_position.z_valid || !vehicle_local_position.v_z_valid) {
-		// there is no altitude limitation present or the required information not available
-		return;
-	}
-
-	// maximum altitude == minimal z-value (NED)
-	const float min_z = _home_position_sub.get().z + (-_param_lndmc_alt_max.get());
-
-	if (vehicle_local_position.z < min_z) {
-		// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
-		setpoint.position[2] = min_z;
-		setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f);
-	}
-}
-
 FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 {
 	// switch to the running task, nothing to do

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -90,7 +90,6 @@ private:
 	void start_flight_task();
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
-	void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
 
 	/**
 	 * Switch to a specific task (for normal usage)
@@ -143,7 +142,6 @@ private:
 	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
@@ -155,7 +153,6 @@ private:
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
 		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
 	);
 };

src/modules/land_detector/land_detector_params_mc.c

@@ -87,24 +87,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f);
  */
 PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
 
-/**
- * Maximum altitude for multicopters
- *
- * The system will obey this limit as a
- * hard altitude limit. This setting will
- * be consolidated with the GF_MAX_VER_DIST
- * parameter.
- * A negative value indicates no altitude limitation.
- *
- * @unit m
- * @min -1
- * @max 10000
- * @decimal 2
- * @group Land Detector
- *
- */
-PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);
-
 /**
  * Ground effect altitude for multicopters
  *

src/modules/navigator/land.cpp

@@ -58,7 +58,6 @@ 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;
 

src/modules/navigator/loiter.cpp

@@ -119,7 +119,6 @@ Loiter::set_loiter_position()
 
 	// convert mission item to current setpoint
 	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

@@ -222,7 +222,6 @@ void Mission::setActiveMissionItems()
 
 		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
 		if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) {
-			mission_apply_limitation(_mission_item);
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}
 
@@ -256,13 +255,11 @@ void Mission::setActiveMissionItems()
 
 		if (num_found_items > 0u) {
 			// We have a position, convert it to the setpoint and update setpoint triplet
-			mission_apply_limitation(next_mission_items[0u]);
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
 		}
 
 		if (num_found_items >= 2u) {
 			/* got next mission item, update setpoint triplet */
-			mission_apply_limitation(next_mission_items[1u]);
 			mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next);
 
 		} else {
@@ -439,7 +436,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
 		set_align_mission_item(&_mission_item, &next_mission_items[0u]);
 
 		/* set position setpoint to target during the transition */
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
 	}
 

src/modules/navigator/mission_base.cpp

@@ -285,7 +285,6 @@ MissionBase::on_active()
 			}
 		}
 
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
 
 		reset_mission_item_reached();
@@ -490,7 +489,6 @@ void MissionBase::setEndOfMissionItems()
 
 	/* 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;
 
@@ -808,7 +806,6 @@ MissionBase::do_abort_landing()
 	_mission_item.autocontinue = false;
 	_mission_item.origin = ORIGIN_ONBOARD;
 
-	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
 
 	// XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during

src/modules/navigator/mission_block.cpp

@@ -877,29 +877,6 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 	item->autocontinue = true;
 }
 
-void
-MissionBlock::mission_apply_limitation(mission_item_s &item)
-{
-	// Limit altitude
-	const float maximum_altitude = _navigator->get_lndmc_alt_max();
-
-	/* do nothing if altitude max is negative */
-	if (maximum_altitude > 0.0f) {
-
-		/* absolute altitude */
-		float altitude_abs = item.altitude_is_relative
-				     ? item.altitude + _navigator->get_home_position()->alt
-				     : item.altitude;
-
-		/* limit altitude to maximum allowed altitude */
-		if ((maximum_altitude + _navigator->get_home_position()->alt) < altitude_abs) {
-			item.altitude = item.altitude_is_relative ?
-					maximum_altitude :
-					maximum_altitude + _navigator->get_home_position()->alt;
-		}
-	}
-}
-
 float
 MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
 {

src/modules/navigator/mission_block.h

@@ -205,11 +205,6 @@ protected:
 	 */
 	void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode);
 
-	/**
-	 * General function used to adjust the mission item based on vehicle specific limitations
-	 */
-	void mission_apply_limitation(mission_item_s &item);
-
 	void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
 				       HeadingMode heading_mode) const;
 

src/modules/navigator/navigator.h

@@ -269,7 +269,6 @@ public:
 	int  get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
 	float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
 	float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
-	float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
 
 	float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
 
@@ -407,7 +406,6 @@ private:
 		(ParamFloat<px4::params::MIS_YAW_TMT>)     _param_mis_yaw_tmt,
 		(ParamFloat<px4::params::MIS_YAW_ERR>)     _param_mis_yaw_err,
 		(ParamFloat<px4::params::MIS_PD_TO>)       _param_mis_payload_delivery_timeout,
-		(ParamFloat<px4::params::LNDMC_ALT_MAX>)   _param_lndmc_alt_max,
 		(ParamInt<px4::params::MIS_LND_ABRT_ALT>)  _param_mis_lnd_abrt_alt
 	)
 };

src/modules/navigator/rtl_direct.cpp

@@ -349,8 +349,6 @@ void RtlDirect::set_rtl_item()
 
 	} else {
 		// Convert mission item to current position setpoint and make it valid.
-		mission_apply_limitation(_mission_item);
-
 		if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
 			_navigator->set_position_setpoint_triplet_updated();
 		}

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -122,7 +122,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
 				      (int32_t)ceilf(_rtl_alt));
 
 		_needs_climbing = false;
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB;
@@ -180,11 +179,9 @@ void RtlDirectMissionLand::setActiveMissionItems()
 		}
 
 		if (num_found_items > 0) {
-			mission_apply_limitation(next_mission_items[0u]);
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 		}
 
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	}
 

src/modules/navigator/rtl_mission_fast.cpp

@@ -132,11 +132,9 @@ void RtlMissionFast::setActiveMissionItems()
 
 
 		if (num_found_items > 0) {
-			mission_apply_limitation(next_mission_items[0u]);
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 		}
 
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	}
 

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -126,12 +126,10 @@ void RtlMissionFastReverse::setActiveMissionItems()
 							       reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
 
 			if (success) {
-				mission_apply_limitation(next_mission_item);
 				mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
 			}
 		}
 
-		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	}
 

src/modules/navigator/takeoff.cpp

@@ -85,8 +85,6 @@ Takeoff::on_active()
 			}
 		}
 
-		mission_apply_limitation(_mission_item);
-
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 		_navigator->set_position_setpoint_triplet_updated();
@@ -129,7 +127,6 @@ 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;

src/modules/navigator/vtol_takeoff.cpp

@@ -74,7 +74,6 @@ VtolTakeoff::on_active()
 				_mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat,
 							    _mission_item.lon, _loiter_location(0), _loiter_location(1)));
 				_mission_item.force_heading = true;
-				mission_apply_limitation(_mission_item);
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 				pos_sp_triplet->current.disable_weather_vane = true;
 				pos_sp_triplet->current.cruising_speed = -1.f;
@@ -180,7 +179,6 @@ VtolTakeoff::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;