ManualSmoothingXY: ensure that maximum speed is correctly propagated to the smoothing class

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

@@ -52,6 +52,7 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
 	/* Smooth velocity setpoint in xy.*/
 	matrix::Vector2f vel(&_velocity(0));
 	Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
+	_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
 	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
 	_velocity_setpoint(0) = vel_sp_xy(0);
 	_velocity_setpoint(1) = vel_sp_xy(1);

src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp

@@ -106,7 +106,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
 		 * Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
 		 * Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */
 		bool yawspeed_demand =  fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
-		bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual.get()) && !yawspeed_demand
+		bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand
 					&& (vel.length() > 2.0f);
 		bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
 
@@ -153,7 +153,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 				if (_jerk_max.get() > _jerk_min.get()) {
 
 					_jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get())
-									  / _vel_manual.get() * vel.length() + _jerk_min.get(), _jerk_max.get());
+									  / _vel_max * vel.length() + _jerk_min.get(), _jerk_max.get());
 				}
 
 				/* Since user wants to brake smoothly but NOT continuing to fly
@@ -195,7 +195,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 	case Intention::acceleration: {
 			/* Limit acceleration linearly based on velocity setpoint.*/
 			_acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get())
-					       / _vel_manual.get() * vel_sp.length() + _dec_xy_min.get();
+					       / _vel_max * vel_sp.length() + _dec_xy_min.get();
 			break;
 		}
 

src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp

@@ -48,6 +48,15 @@ public:
 	ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
 	~ManualSmoothingXY() = default;
 
+	/**
+	 * Maximum velocity is required to detect user intention.
+	 * Maximum velocity changes depending on task. Consequently,
+	 * in order to deduce user intention from velocity, the maximum
+	 * allowed velocity has to updated.
+	 * @param vel_max corresponds to vehicle constraint
+	 */
+	void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;};
+
 	/**
 	 * Smoothing of velocity setpoint horizontally based
 	 * on flight direction.
@@ -97,6 +106,12 @@ private:
 	float _acc_state_dependent{0.0f};
 	float _jerk_state_dependent{0.0f};
 
+	/**
+	 * Maximum velocity.
+	 * It is used to deduce user intention.
+	 */
+	float _vel_max{0.0f};
+
 	/* Previous setpoints */
 	matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint
 
@@ -108,5 +123,4 @@ private:
 		(ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max, ///< jerk max during brake
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_manual ///< maximum velocity in manual controlled mode
 	)
-
 };