FlightTask: introduce method for limits and adjust accordingly for all the tasks

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -17,6 +17,7 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 bool FlightTask::activate()
 {
 	_resetSetpoints();
+	_updateSetpointLimits();
 	_time_stamp_activate = hrt_absolute_time();
 	return true;
 }
@@ -72,3 +73,10 @@ bool FlightTask::_evaluateVehicleLocalPosition()
 		return false;
 	}
 }
+
+void FlightTask::_updateSetpointLimits()
+{
+	_limits.speed_NE_max = MPC_XY_VEL_MAX.get();
+	_limits.speed_up_max = MPC_Z_VEL_MAX_UP.get();
+	_limits.speed_dn_max = MPC_Z_VEL_MAX_DN.get();
+}

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -105,6 +105,35 @@ public:
 	}
 
 protected:
+
+
+	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
+
+	/**
+	 * Reset all setpoints to NAN
+	 */
+	void _resetSetpoints();
+
+	/**
+	 * Update Flighttask limits
+	 */
+	void virtual _updateSetpointLimits();
+
+	/*
+	 * Check and update local position
+	 */
+	bool _evaluateVehicleLocalPosition();
+
+	/**
+	 * @brief Setpoint limits
+	 */
+	struct Limits {
+		float speed_NE_max = 1.0f; /**< maximum speed in NE-direction */
+		float speed_up_max = 1.0f; /**< maximum speed upwards */
+		float speed_dn_max = 1.0f; /**< maximum speed downwards */
+	};
+	Limits _limits;
+
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
 	float _time = 0; /**< passed time in seconds since the task was activated */
@@ -128,11 +157,9 @@ protected:
 	float _yawspeed_setpoint;
 	float _dist_to_bottom;
 
-	/**
-	 * Get the output data
-	 */
-	void _resetSetpoints();
-
-	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
-	bool _evaluateVehicleLocalPosition();
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) MPC_Z_VEL_MAX_UP
+				       );
 };

src/lib/FlightTasks/tasks/FlightTaskAuto.cpp

@@ -94,12 +94,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}
 
 	_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+
 	// always update cruise speed since that can change without waypoint changes
 	_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
 
-	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
-		// use default
-		_mc_cruise_speed = _mc_cruise_default.get();
+	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _limits.speed_NE_max)) {
+		// use default limit
+		_mc_cruise_speed = _limits.speed_NE_max;
 	}
 
 	// get target waypoint.
@@ -184,3 +185,14 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition()
 		_time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp;
 	}
 }
+
+void FlightTaskAuto::_updateSetpointLimits()
+{
+	FlightTask::_updateSetpointLimits();
+
+	// only adjust limits if the new limit is lower
+	if (_limits.speed_NE_max >= MPC_XY_CRUISE.get()) {
+		_limits.speed_NE_max = MPC_XY_CRUISE.get();
+	}
+
+}

src/lib/FlightTasks/tasks/FlightTaskAuto.hpp

@@ -70,6 +70,8 @@ public:
 	bool updateInitialize() override;
 
 protected:
+	void _updateSetpointLimits() override;
+
 	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
 	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */
 	matrix::Vector3f _target{}; /**< Target waypoint  (local frame).*/
@@ -84,7 +86,7 @@ private:
 	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_XY_CRUISE>) _mc_cruise_default); /**< Default mc cruise speed.*/
+					(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE); /**< Default mc cruise speed.*/
 
 	map_projection_reference_s _reference; /**< Reference frame from global to local. */
 

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -126,7 +126,7 @@ void FlightTaskAutoLine::_generateLandSetpoints()
 {
 	// Keep xy-position and go down with landspeed. */
 	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
-	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get()));
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, MPC_LAND_SPEED.get()));
 }
 
 void FlightTaskAutoLine::_generateTakeoffSetpoints()
@@ -183,7 +183,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
 
 			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+			    (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
 
 				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
 					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -206,7 +206,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
 			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+			    (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
 
 				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
 					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -230,7 +230,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
 			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+			    (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
 
 				angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
 					* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
@@ -254,7 +254,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
 			if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) {
+			    (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
 
 				angle =
 					Vector2f(&(_destination - _origin)(0)).unit_or_zero()
@@ -269,10 +269,10 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
 void FlightTaskAutoLine::_generateXYsetpoints()
 {
 	Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
-	const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < _nav_rad.get();
+	const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < NAV_ACC_RAD.get();
 
-	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) ||
-	    (!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) {
+	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
+	    (!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) {
 
 		// Vehicle reached target in xy and no passing required. Lock position */
 		_position_setpoint(0) = _destination(0);
@@ -302,9 +302,9 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 		}
 
 		// Compute maximum speed at target threshold */
-		if (threshold_max > _nav_rad.get()) {
-			float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _nav_rad.get());
-			speed_threshold = m * (target_threshold - _nav_rad.get()) + _speed_at_target; // speed at transition
+		if (threshold_max > NAV_ACC_RAD.get()) {
+			float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get());
+			speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition
 		}
 
 		// Either accelerate or decelerate
@@ -317,7 +317,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 				_speed_at_target = 0.0f;
 			}
 
-			float acceptance_radius = _nav_rad.get();
+			float acceptance_radius = NAV_ACC_RAD.get();
 
 			if (_speed_at_target < 0.01f) {
 				// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
@@ -355,7 +355,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 			}
 
 			// If yaw offset is large, only accelerate with 0.5 m/s^2.
-			float acc_max = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acc_xy.get();
+			float acc_max = (fabsf(yaw_diff) > math::radians(MIS_YAW_ERR.get())) ? 0.5f : MPC_ACC_HOR.get();
 
 			if (acc_track > acc_max) {
 				// accelerate towards target
@@ -392,12 +392,12 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
 		// limit vertical downwards speed (positive z) close to ground
 		// for now we use the altitude above home and assume that we want to land at same height as we took off
 		float vel_limit = math::gradual(_alt_above_ground,
-						_slow_land_alt2.get(), _slow_land_alt1.get(),
-						_land_speed.get(), _vel_max_down.get());
+						MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
+						MPC_LAND_SPEED.get(), _limits.speed_dn_max);
 
 		// Speed at threshold is by default maximum speed. Threshold defines
 		// the point in z at which vehicle slows down to reach target altitude.
-		float speed_sp = (flying_upward) ? _vel_max_up.get() : vel_limit;
+		float speed_sp = (flying_upward) ? _limits.speed_up_max : vel_limit;
 
 		// Target threshold defines the distance to target(2) at which
 		// the vehicle starts to slow down to approach the target smoothly.
@@ -426,7 +426,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
 			// we want to accelerate
 
 			const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
-			const float acc_max = (flying_upward) ? (_acc_max_up.get() * 0.5f) : (_acc_max_down.get() * 0.5f);
+			const float acc_max = (flying_upward) ? (MPC_ACC_UP_MAX.get() * 0.5f) : (MPC_ACC_DOWN_MAX.get() * 0.5f);
 
 			if (acc > acc_max) {
 				speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
@@ -463,7 +463,7 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
 
 	// Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees.
 	// It needs to be always larger than minimum cruise speed.
-	float middle_cruise_speed = _cruise_speed_90.get();
+	float middle_cruise_speed = MPC_CRUISE_90.get();
 
 	if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
 		middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
@@ -514,5 +514,5 @@ void FlightTaskAutoLine::updateParams()
 	FlightTaskAuto::updateParams();
 
 	// make sure that alt1 is above alt2
-	_slow_land_alt1.set(math::max(_slow_land_alt1.get(), _slow_land_alt2.get()));
+	MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
 }

src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp

@@ -66,18 +66,15 @@ protected:
 	State _current_state{State::none};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _land_speed,
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up,
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down,
-					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _acc_max_xy,
-					(ParamFloat<px4::params::MPC_ACC_HOR>) _acc_xy,
-					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _acc_max_up,
-					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
-					(ParamFloat<px4::params::MPC_CRUISE_90>) _cruise_speed_90,
-					(ParamFloat<px4::params::NAV_ACC_RAD>) _nav_rad,
-					(ParamFloat<px4::params::MIS_YAW_ERR>) _mis_yaw_error,
-					(ParamFloat<px4::params::MPC_LAND_ALT1>) _slow_land_alt1,
-					(ParamFloat<px4::params::MPC_LAND_ALT2>) _slow_land_alt2
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
+					(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees
+					(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated
+					(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR // acceleration in flight
 				       )
 
 	void _generateIdleSetpoints();

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -46,7 +46,7 @@ void FlightTaskManualAltitude::_scaleSticks()
 	FlightTaskManualStabilized::_scaleSticks();
 
 	/* Scale horizontal velocity with expo curve stick input*/
-	const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
+	const float vel_max_z = (_sticks(2) > 0.0f) ? _limits.speed_dn_max : _limits.speed_up_max;
 	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
 
@@ -58,7 +58,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 	/* handle position and altitude hold */
 	const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
-	const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get());
+	const bool stopped_z = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get());
 
 	if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
 		_position_setpoint(2) = _position(2);

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp

@@ -54,10 +54,7 @@ protected:
 	void _scaleSticks() override; /**< scales sticks to velocity in z */
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down, /**< maximum speed allowed to go up */
-					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up, /**< maximum speed allowed to go down */
-					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>)
-					_vel_hold_thr_z /**< velocity threshold to switch back into vertical position hold */
+					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z
 				       )
 
 };

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -55,8 +55,8 @@ void FlightTaskManualPosition::_scaleSticks()
 		stick_xy = stick_xy.normalized() * mag;
 	}
 
-	/* Scale to velocity.*/
-	Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
+	// scale velocity to its maximum lmits
+	Vector2f vel_sp_xy = stick_xy * _limits.speed_NE_max;
 
 	/* Rotate setpoint into local frame. */
 	_rotateIntoHeadingFrame(vel_sp_xy);
@@ -69,7 +69,7 @@ void FlightTaskManualPosition::_updateXYlock()
 	/* If position lock is not active, position setpoint is set to NAN.*/
 	const float vel_xy_norm = Vector2f(&_velocity(0)).length();
 	const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
-	const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
+	const bool stopped = (MPC_HOLD_MAX_XY.get() < FLT_EPSILON || vel_xy_norm < MPC_HOLD_MAX_XY.get());
 
 	if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
 		_position_setpoint(0) = _position(0);
@@ -88,3 +88,12 @@ void FlightTaskManualPosition::_updateSetpoints()
 	_thrust_setpoint *= NAN; // don't require any thrust setpoints
 	_updateXYlock(); // check for position lock
 }
+
+void FlightTaskManualPosition::_updateSetpointLimits()
+{
+	FlightTaskManualAltitude::_updateSetpointLimits();
+
+	if (_limits.speed_NE_max >= MPC_VEL_MANUAL.get()) {
+		_limits.speed_NE_max = MPC_VEL_MANUAL.get();
+	}
+}

src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp

@@ -53,14 +53,12 @@ protected:
 	void _updateXYlock(); /**< applies positon lock based on stick and velocity */
 	void _updateSetpoints() override;
 	void _scaleSticks() override;
+	void _updateSetpointLimits() override;
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
-					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_xy_manual_max, /**< maximum speed allowed horizontally */
-					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>)
-					_acc_xy_max, /**< maximum acceleration horizontally. Only used to compute lock time */
-					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>)
-					_vel_hold_thr_xy /**< velocity threshold to switch back into horizontal position hold */
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) MPC_VEL_MANUAL,
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY
 				       )
-private:
 
 };

src/lib/FlightTasks/tasks/FlightTaskSport.hpp

@@ -53,23 +53,11 @@ public:
 	virtual ~FlightTaskSport() = default;
 
 protected:
-	void _updateSetpoints() override
+	void _updateSetpointLimits() override
 	{
-		FlightTaskManualPosition::_updateSetpoints(); // get all setpoints from position task
+		FlightTaskManualPosition::_updateSetpointLimits();
 
-		/* Scale horizontal velocity setpoint by maximum allowed velocity. */
-		if (PX4_ISFINITE(_velocity_setpoint(0)) && Vector2f(&_velocity_setpoint(0)).length() > 0.0f) {
-			Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
-			vel_sp_xy = vel_sp_xy.normalized() * _vel_xy_max.get() / _vel_xy_manual_max.get() * vel_sp_xy.length();
-			_velocity_setpoint(0) = vel_sp_xy(0);
-			_velocity_setpoint(1) = vel_sp_xy(1);
-		}
+		// for sport mode we just increase horizontal speed to maximum speed
+		_limits.speed_NE_max = MPC_XY_VEL_MAX.get();
 	}
-
-private:
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
-					(ParamFloat<px4::params::MPC_XY_VEL_MAX>)
-					_vel_xy_max /**< maximal allowed horizontal speed, in sport mode full stick input*/
-				       )
-
 };