Remove relaying of maximum altitude through land detector

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -64,7 +64,6 @@ param set-default FW_BAT_SCALE_EN 1
 param set-default FW_THR_ALT_SCL 2.7
 param set-default FW_T_RLL2THR 20
 
-param set-default LNDMC_ALT_MAX 9999
 param set-default LNDMC_XY_VEL_MAX 1
 param set-default LNDMC_Z_VEL_MAX 0.7
 

msg/vehicle_land_detected.msg

@@ -1,5 +1,4 @@
 uint64 timestamp	# time since system start (microseconds)
-float32 alt_max 	# maximum altitude in [m] that can be reached
 bool freefall		# true if vehicle is currently in free-fall
 bool ground_contact	# true if vehicle has ground contact but is not landed (1. stage)
 bool maybe_landed	# true if the vehicle might have landed (2. stage)

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -501,14 +501,14 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
 				      const vehicle_local_position_s &vehicle_local_position)
 {
-	if (_vehicle_land_detected_sub.get().alt_max < 0.0f || !_home_position_sub.get().valid_alt
+	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 + (-_vehicle_land_detected_sub.get().alt_max);
+	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)

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -158,6 +158,7 @@ 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/LandDetector.cpp

@@ -122,7 +122,6 @@ void LandDetector::Run()
 	const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
 	const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
 	const bool landDetected = _landed_hysteresis.get_state();
-	const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY;
 	const bool in_ground_effect = _ground_effect_hysteresis.get_state();
 
 	// publish at 1 Hz, very first time, or when the result has changed
@@ -131,8 +130,7 @@ void LandDetector::Run()
 	    (_land_detected.freefall != freefallDetected) ||
 	    (_land_detected.maybe_landed != maybe_landedDetected) ||
 	    (_land_detected.ground_contact != ground_contactDetected) ||
-	    (_land_detected.in_ground_effect != in_ground_effect) ||
+	    (_land_detected.in_ground_effect != in_ground_effect)) {
-	    (fabsf(_land_detected.alt_max - alt_max) > FLT_EPSILON)) {
 
 		if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
 			// We did take off
@@ -143,7 +141,6 @@ void LandDetector::Run()
 		_land_detected.freefall = freefallDetected;
 		_land_detected.maybe_landed = maybe_landedDetected;
 		_land_detected.ground_contact = ground_contactDetected;
-		_land_detected.alt_max = alt_max;
 		_land_detected.in_ground_effect = in_ground_effect;
 		_land_detected.in_descend = _get_in_descend();
 		_land_detected.has_low_throttle = _get_has_low_throttle();

src/modules/land_detector/LandDetector.h

@@ -126,11 +126,6 @@ protected:
 	 */
 	virtual bool _get_freefall_state() { return false; }
 
-	/**
-	 *  @return maximum altitude that can be reached
-	 */
-	virtual float _get_max_altitude() { return INFINITY; }
-
 	/**
 	 *  @return true if vehicle could be in ground effect (close to ground)
 	 */
@@ -163,7 +158,6 @@ private:
 
 	vehicle_land_detected_s _land_detected = {
 		.timestamp = 0,
-		.alt_max = -1.0f,
 		.freefall = false,
 		.ground_contact = true,
 		.maybe_landed = true,

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -323,16 +323,6 @@ bool MulticopterLandDetector::_get_landed_state()
 	return _maybe_landed_hysteresis.get_state();
 }
 
-float MulticopterLandDetector::_get_max_altitude()
-{
-	if (_param_lndmc_alt_max.get() < 0.0f) {
-		return INFINITY;
-
-	} else {
-		return _param_lndmc_alt_max.get();
-	}
-}
-
 float MulticopterLandDetector::_get_gnd_effect_altitude()
 {
 	if (_param_lndmc_alt_gnd_effect.get() < 0.0f) {

src/modules/land_detector/MulticopterLandDetector.h

@@ -76,7 +76,6 @@ protected:
 	bool _get_horizontal_movement() override { return _horizontal_movement; }
 	bool _get_vertical_movement() override { return _vertical_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
-	float _get_max_altitude() override;
 
 	void _set_hysteresis_factor(const int factor) override;
 private:
@@ -140,7 +139,6 @@ private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		LandDetector,
 		(ParamFloat<px4::params::LNDMC_TRIG_TIME>)   _param_lndmc_trig_time,
-		(ParamFloat<px4::params::LNDMC_ALT_MAX>)    _param_lndmc_alt_max,
 		(ParamFloat<px4::params::LNDMC_ROT_MAX>)    _param_lndmc_rot_max,
 		(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
 		(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>)  _param_lndmc_z_vel_max,

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -120,7 +120,6 @@ private:
 
 	vehicle_land_detected_s _vehicle_land_detected {
 		.timestamp = 0,
-		.alt_max = -1.0f,
 		.freefall = false,
 		.ground_contact = true,
 		.maybe_landed = true,

src/modules/navigator/mission_block.cpp

@@ -770,12 +770,11 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 void
 MissionBlock::mission_apply_limitation(mission_item_s &item)
 {
-	/*
+	// Limit altitude
-	 * Limit altitude
+	const float maximum_altitude = _navigator->get_lndmc_alt_max();
-	 */
 
 	/* do nothing if altitude max is negative */
-	if (_navigator->get_land_detected()->alt_max > 0.0f) {
+	if (maximum_altitude > 0.0f) {
 
 		/* absolute altitude */
 		float altitude_abs = item.altitude_is_relative
@@ -783,17 +782,12 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 				     : item.altitude;
 
 		/* limit altitude to maximum allowed altitude */
-		if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
+		if ((maximum_altitude + _navigator->get_home_position()->alt) < altitude_abs) {
 			item.altitude = item.altitude_is_relative ?
-					_navigator->get_land_detected()->alt_max :
+					maximum_altitude :
-					_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
+					maximum_altitude + _navigator->get_home_position()->alt;
-
 		}
 	}
-
-	/*
-	 * Add other limitations here
-	 */
 }
 
 float

src/modules/navigator/navigator.h

@@ -299,6 +299,7 @@ public:
 	bool		get_takeoff_required() const { return _param_mis_takeoff_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; }
 	float		get_vtol_reverse_delay() const { return _param_reverse_delay; }
@@ -329,7 +330,10 @@ private:
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
 		(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
 		(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
-		(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
+		(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
+
+		(ParamFloat<px4::params::LNDMC_ALT_MAX>)    _param_lndmc_alt_max
+
 	)
 
 	struct traffic_buffer_s {