max hagl vel restriction in ManAcc position mode

msg/VehicleLocalPosition.msg

@@ -75,11 +75,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_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)
+# 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

@@ -329,6 +329,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.vz_max = INFINITY;
 			local_position.hagl_min = 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_helper.cpp

@@ -362,17 +362,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		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_xy = flow_hagl_max;

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

@@ -59,9 +59,40 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 
 bool FlightTaskManualAcceleration::update()
 {
-	setMaxDistanceToGround(_sub_vehicle_local_position.get().hagl_max_xy);
+	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)) {
+		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
+	const float factor_threshold = 0.8f; // threshold ratio of max_hagl
+	const 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);
+		float flow_vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
+
+		flow_vxy_max = flow_vxy_max + (max_hagl_ratio - factor_threshold) * (min_vel - flow_vxy_max) /
+			       (min_vel - factor_threshold);
+
+		const float current_vel_constraint = _stick_acceleration_xy.getVelocityConstraint();
+
+		if (abs(current_vel_constraint - flow_vxy_max) > 0.1f) {
+			// adjust velocity constraint with a lag because good tracking is required for the drag estimation
+			const float v_limit = math::constrain(flow_vxy_max, current_vel_constraint - _deltatime * _param_mpc_acc_hor.get(),
+							      current_vel_constraint + _deltatime * _param_mpc_acc_hor.get());
+			_stick_acceleration_xy.setVelocityConstraint(v_limit);
+		}
+
+	} 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/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -231,8 +231,12 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 		    && _velocity_setpoint(2) <= 0.f) {
 
 			_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());
+			_velocity_setpoint(2) = NAN;
+
+			if (_dist_to_bottom > _max_distance_to_ground) {
+				_velocity_setpoint(2) = math::constrain(_param_mpc_z_p.get() * (_dist_to_bottom - _max_distance_to_ground),
+									0.f, _param_mpc_z_vel_max_dn.get());
+			}
 		}
 	}
 }

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

@@ -91,7 +91,9 @@ class FlightTaskManualAltitude : public FlightTask
 					(ParamFloat<px4::params::MPC_LAND_SPEED>)
 					_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
 					(ParamFloat<px4::params::MPC_TKO_SPEED>)
-					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
+					_param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
 				       )
 private:
 	/**

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

@@ -63,6 +63,7 @@ class StickAccelerationXY : public ModuleParams
 	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); };
+	float getVelocityConstraint() { return _velocity_constraint; };
 
 private:
 	void applyJerkLimit(const float dt);

src/modules/navigator/mission_block.cpp

@@ -1066,7 +1066,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_xy)) {
+	    && ((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");