fix max-hagl restriction to position/altitude control

msg/VehicleLocalPosition.msg

@@ -78,7 +78,8 @@ bool dead_reckoning                     # True if this position is estimated thr
 float32 vxy_max				# maximum horizontal speed - set to 0 when limiting not required (meters/sec)
 float32 vz_max				# maximum vertical speed - set to 0 when limiting not required (meters/sec)
 float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
-float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
+float32 hagl_max_z			# maximum height above ground level for z-control - set to 0 when limiting not required (meters)
+float32 hagl_max_xy			# maximum height above ground level for xy-control - set to 0 when limiting not required (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
 # TOPICS estimator_local_position

src/drivers/ins/vectornav/VectorNav.cpp

@@ -328,7 +328,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.vxy_max = INFINITY;
 			local_position.vz_max = INFINITY;
 			local_position.hagl_min = INFINITY;
-			local_position.hagl_max = INFINITY;
+			local_position.hagl_max_z = INFINITY;
 
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();

src/modules/ekf2/EKF/ekf.h

@@ -280,8 +280,9 @@ class Ekf final : public EstimatorInterface
 	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
 	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
 	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
-	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
+	//  hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed.
+	//  hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed.
+	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const;
 
 	void resetGyroBias();
 	void resetGyroBiasCov();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -318,13 +318,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }
 
-void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
+void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z,
+			      float *hagl_max_xy) const
 {
 	// Do not require limiting by default
 	*vxy_max = NAN;
 	*vz_max = NAN;
 	*hagl_min = NAN;
-	*hagl_max = NAN;
+	*hagl_max_z = NAN;
+	*hagl_max_xy = NAN;
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
@@ -339,7 +341,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 	// Keep within range sensor limit when using rangefinder as primary height source
 	if (relying_on_rangefinder) {
 		*hagl_min = rangefinder_hagl_min;
-		*hagl_max = rangefinder_hagl_max;
+		*hagl_max_z = rangefinder_hagl_max;
 	}
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -362,11 +364,23 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
 
 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
-		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f);
+
+		if (!control_status_flags().fixed_wing) {
+			float max_hagl_factor = (_state.terrain - _state.pos(2)) / flow_hagl_max;
+
+			// limit horizontal velocity near max hagl to decrease chance of large distance to ground jumps
+			if (max_hagl_factor > 0.9f) {
+				max_hagl_factor = math::min(max_hagl_factor, 1.f);
+				flow_vxy_max = flow_vxy_max + (max_hagl_factor - 0.9f) * (1.f - flow_vxy_max) * 10.f;
+
+			}
+		}
 
 		*vxy_max = flow_vxy_max;
 		*hagl_min = flow_hagl_min;
-		*hagl_max = flow_hagl_max;
+		*hagl_max_xy = flow_hagl_max;
 	}
 
 # endif // CONFIG_EKF2_OPTICAL_FLOW

src/modules/ekf2/EKF2.cpp

@@ -1627,7 +1627,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 			      || _ekf.control_status_flags().wind_dead_reckoning;
 
 	// get control limit information
-	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
+	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy);
 
 	// convert NaN to INFINITY
 	if (!PX4_ISFINITE(lpos.vxy_max)) {
@@ -1642,8 +1642,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.hagl_min = INFINITY;
 	}
 
-	if (!PX4_ISFINITE(lpos.hagl_max)) {
-		lpos.hagl_max = INFINITY;
+	if (!PX4_ISFINITE(lpos.hagl_max_z)) {
+		lpos.hagl_max_z = INFINITY;
+	}
+
+	if (!PX4_ISFINITE(lpos.hagl_max_xy)) {
+		lpos.hagl_max_xy = INFINITY;
 	}
 
 	// publish vehicle local position data

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -59,6 +59,7 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 
 bool FlightTaskManualAcceleration::update()
 {
+	setMaxDistanceToGround(_sub_vehicle_local_position.get().hagl_max_xy);
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 		_min_distance_to_ground = -INFINITY;
 	}
 
-	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
-		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
-
-	} else {
-		_max_distance_to_ground = INFINITY;
+	if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) {
+		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z;
 	}
 }
 
@@ -154,8 +151,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
-		// respect maximum altitude
-		_respectMaxAltitude();
 
 	} else {
 		// normal mode where height is dependent on local frame
@@ -185,9 +180,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			// user demands velocity change
 			_position_setpoint(2) = NAN;
 			// ensure that maximum altitude is respected
-			_respectMaxAltitude();
 		}
 	}
+
+	_respectMaxAltitude();
 }
 
 void FlightTaskManualAltitude::_respectMinAltitude()
@@ -229,28 +225,14 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 {
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 
-		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
-		// below the maximum, preserving control loop vertical position error gain.
-		// TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller
-		if (PX4_ISFINITE(_max_distance_to_ground)) {
-			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
-								-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
-
-		} else {
-			_constraints.speed_up = _param_mpc_z_vel_max_up.get();
-		}
+		if ((-_position_setpoint(2) + _position(2) > _max_distance_to_ground - _dist_to_bottom
+		     || -_velocity_setpoint(2) > _max_distance_to_ground - _dist_to_bottom
+		     || _dist_to_bottom > _max_distance_to_ground)
+		    && _velocity_setpoint(2) <= 0.f) {
 
-		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
-		if (_dist_to_bottom > _max_distance_to_ground) {
-			// difference between current distance to ground and maximum distance to ground
-			const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground;
-			// set position setpoint to maximum distance to ground
-			_position_setpoint(2) = _position(2) + delta_distance_to_max;
-			// limit speed downwards to 0.7m/s
-			_constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f);
-
-		} else {
-			_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+			_position_setpoint(2) = _position(2) - _max_distance_to_ground + _dist_to_bottom;
+			_velocity_setpoint(2) = - math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
+						-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
 		}
 	}
 }
@@ -301,6 +283,7 @@ bool FlightTaskManualAltitude::update()
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
+	_max_distance_to_ground = INFINITY;
 
 	return ret;
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -53,6 +53,7 @@ class FlightTaskManualAltitude : public FlightTask
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
+	void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; }
 
 protected:
 	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vxy_max = INFINITY;
 		_pub_lpos.get().vz_max = INFINITY;
 		_pub_lpos.get().hagl_min = INFINITY;
-		_pub_lpos.get().hagl_max = INFINITY;
+		_pub_lpos.get().hagl_max_z = INFINITY;
+		_pub_lpos.get().hagl_max_xy = INFINITY;
 		_pub_lpos.get().timestamp = hrt_absolute_time();;
 		_pub_lpos.update();
 	}

src/modules/mavlink/mavlink_receiver.cpp

@@ -2729,7 +2729,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vxy_max = INFINITY;
 		hil_local_pos.vz_max = INFINITY;
 		hil_local_pos.hagl_min = INFINITY;
-		hil_local_pos.hagl_max = INFINITY;
+		hil_local_pos.hagl_max_z = INFINITY;
+		hil_local_pos.hagl_max_xy = INFINITY;
 		hil_local_pos.timestamp = hrt_absolute_time();
 		_local_pos_pub.publish(hil_local_pos);
 	}

src/modules/navigator/mission_block.cpp

@@ -1057,7 +1057,7 @@ void MissionBlock::updateMaxHaglFailsafe()
 	const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
 
 	if (_navigator->get_global_position()->terrain_alt_valid
-	    && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
+	    && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max_xy)) {
 		// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
 		events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -609,7 +609,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 		hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
 		hil_lpos.vz_max = std::numeric_limits<float>::infinity();
 		hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
-		hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_max_z = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_max_xy = std::numeric_limits<float>::infinity();
 
 		// always publish ground truth attitude message
 		_lpos_ground_truth_pub.publish(hil_lpos);