simplyfied vertical vel limitation

PX4 · Jan 16, 2025 · 47ad0a3 · 47ad0a3
1 parent 3209e2b
commit 47ad0a3
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 13 deletions.
diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/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
+				       )
 };
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -225,19 +225,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 {
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 
-		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) {
+		float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom);
 
-			_position_setpoint(2) = _position(2) - _max_distance_to_ground + _dist_to_bottom;
-			_velocity_setpoint(2) = NAN;
+		if (PX4_ISFINITE(_max_distance_to_ground)) {
+			_constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
 
-			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());
-			}
+		} else {
+			_constraints.speed_up = _param_mpc_z_vel_max_up.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();
 	}
 }
 

diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -91,9 +91,7 @@ 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 */
-					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
-					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
+					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
 				       )
 private:
 	/**