Fix max-hagl restriction to position/altitude control (#23667)

* fix max-hagl restriction to position/altitude control

* max hagl vel restriction in ManAcc position mode

* use interpolate func, change naming

* simplyfied vertical vel limitation

* move velocity-constraint adjustment to StickAccelXY

Author: haumarco

msg/versioned/VehicleLocalPosition.msg

@@ -77,10 +77,12 @@ float32 evv				# Standard deviation of vertical velocity error, (metres/sec)
 bool dead_reckoning                     # True if this position is estimated through dead-reckoning
 
 # estimator specified vehicle limits
-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)
+# set to INFINITY when limiting not required
+float32 vxy_max				# maximum horizontal speed (meters/sec)
+float32 vz_max				# maximum vertical speed (meters/sec)
+float32 hagl_min			# minimum height above ground level (meters)
+float32 hagl_max_z			# maximum height above ground level for z-control (meters)
+float32 hagl_max_xy			# maximum height above ground level for xy-control (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,8 @@ 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.hagl_max_xy = INFINITY;
 
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();

src/modules/ekf2/EKF/ekf.h

@@ -208,8 +208,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

@@ -327,13 +327,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
@@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 
 	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)
@@ -370,11 +372,12 @@ 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);
 
 		*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

@@ -1628,7 +1628,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)) {
@@ -1643,8 +1643,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,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 
 bool FlightTaskManualAcceleration::update()
 {
+	const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get();
+	setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy);
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
+	float max_hagl_ratio = 0.0f;
+
+	if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) {
+		max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy;
+	}
+
+	// limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps
+	static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl
+	static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
+
+	if (max_hagl_ratio > factor_threshold) {
+		max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
+		const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
+		_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
+
+	} else {
+		_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
+	}
+
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);

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

@@ -52,4 +52,9 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 
 	StickAccelerationXY _stick_acceleration_xy{this};
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
+				       )
 };

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,29 +225,20 @@ 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
+		float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom);
+
 		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());
+			_constraints.speed_up = math::constrain(vel_constrained, -_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 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();
+		if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) {
+			_velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get());
 		}
+
+		_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
 	}
 }
 
@@ -306,6 +293,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/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
 		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
+	// gradually adjust velocity constraint because good tracking is required for the drag estimation
+	if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) {
+		if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) {
+			_current_velocity_constraint = _targeted_velocity_constraint;
+
+		} else {
+			_current_velocity_constraint = math::constrain(_targeted_velocity_constraint,
+						       _current_velocity_constraint - dt * _param_mpc_acc_hor.get(),
+						       _current_velocity_constraint + dt * _param_mpc_acc_hor.get());
+		}
+	}
+
 	// maximum commanded velocity can be constrained dynamically
-	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
+	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint);
 	Vector2f velocity_scale(velocity_sc, velocity_sc);
 	// maximum commanded acceleration is scaled down with velocity
 	const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -63,7 +63,8 @@ class StickAccelerationXY : public ModuleParams
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
 	float getMaxAcceleration() { return _param_mpc_acc_hor.get(); };
 	float getMaxJerk() { return _param_mpc_jerk_max.get(); };
-	void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	float getVelocityConstraint() { return _current_velocity_constraint; };
 
 private:
 	CollisionPrevention _collision_prevention{this};
@@ -85,7 +86,8 @@ class StickAccelerationXY : public ModuleParams
 	matrix::Vector2f _acceleration_setpoint;
 	matrix::Vector2f _acceleration_setpoint_prev;
 
-	float _velocity_constraint{INFINITY};
+	float _targeted_velocity_constraint{INFINITY};
+	float _current_velocity_constraint{INFINITY};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,

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

@@ -2738,7 +2738,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

@@ -1026,7 +1026,8 @@ 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)
+		> math::min(_navigator->get_local_position()->hagl_max_z, _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

@@ -607,7 +607,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);