Auto mode traj - limit yaw setpoint rate of change when generated in the flight task instead of clamping the yaw rate in the controller

Move yaw setpoint slew rate from AutoLineSmoothVel to Auto. The slew rate is now applied consistently to all the auto FlightTasks

src/lib/FlightTasks/FlightTasks.cpp

@@ -21,7 +21,7 @@ bool FlightTasks::update()
 
 	if (isAnyTaskActive()) {
 		_subscription_array.update();
-		return _current_task.task->updateInitialize() && _current_task.task->update();
+		return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize();
 	}
 
 	return false;

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -78,7 +78,7 @@ bool FlightTaskAuto::activate()
 	bool ret = FlightTask::activate();
 	_position_setpoint = _position;
 	_velocity_setpoint = _velocity;
-	_yaw_setpoint = _yaw;
+	_yaw_setpoint = _yaw_sp_prev = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	_setDefaultConstraints();
 	return ret;
@@ -100,6 +100,44 @@ bool FlightTaskAuto::updateInitialize()
 	return ret;
 }
 
+bool FlightTaskAuto::updateFinalize()
+{
+	// All the auto FlightTasks have to comply with defined maximum yaw rate
+	// If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
+	// if will see its setpoint constrained here
+	_limitYawRate();
+	return true;
+}
+
+void FlightTaskAuto::_limitYawRate()
+{
+	if (PX4_ISFINITE(_yaw_setpoint) || PX4_ISFINITE(_yaw_sp_prev)) {
+		// Limit the rate of change of the yaw setpoint
+		float dy_max = math::radians(_param_mc_yawrauto_max.get()) * _deltatime;
+
+		if (fabsf(_yaw_setpoint - _yaw_sp_prev) < M_PI_F) {
+			// Wrap around 0
+			_yaw_setpoint = math::constrain(_yaw_setpoint,
+							_yaw_sp_prev - dy_max,
+							_yaw_sp_prev + dy_max);
+
+		} else {
+			// Wrap around PI/-PI
+			_yaw_setpoint = matrix::wrap_pi(
+						math::constrain(matrix::wrap_2pi(_yaw_setpoint),
+								matrix::wrap_2pi(_yaw_sp_prev) - dy_max,
+								matrix::wrap_2pi(_yaw_sp_prev) + dy_max)
+					);
+		}
+
+		_yaw_sp_prev = _yaw_setpoint;
+	}
+
+	if (PX4_ISFINITE(_yawspeed_setpoint)) {
+		_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -_param_mc_yawrauto_max.get(), _param_mc_yawrauto_max.get());
+	}
+}
+
 bool FlightTaskAuto::_evaluateTriplets()
 {
 	// TODO: fix the issues mentioned below

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -79,6 +79,7 @@ public:
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate() override;
 	bool updateInitialize() override;
+	bool updateFinalize() override;
 
 	/**
 	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
@@ -111,12 +112,14 @@ protected:
 					(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
 					_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
 					(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
-					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid // obstacle avoidance active
+					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
+					(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _param_mc_yawrauto_max
 				       );
 
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
 	bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
+	float _yaw_sp_prev = NAN;
 	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
 	uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr};
 
@@ -136,6 +139,7 @@ private:
 		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
 
+	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
 	bool _evaluateTriplets(); /**< Checks and sets triplets. */
 	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
 	bool _evaluateGlobalReference(); /**< Check is global reference is available. */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -105,6 +105,14 @@ public:
 	 */
 	virtual bool update() = 0;
 
+	/**
+	 * Call after update()
+	 * to constrain the generated setpoints in order to comply
+	 * with the constraints of the current mode
+	 * @return true on success, false on error
+	 */
+	virtual bool updateFinalize() { return true; };
+
 	/**
 	 * Get the output data
 	 * @return task output setpoints that get executed by the positon controller

src/modules/mc_att_control/mc_att_control.hpp

@@ -147,9 +147,6 @@ private:
 	 */
 	matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate);
 
-	/** lower yawspeed limit in auto modes because we expect yaw steps */
-	void adapt_auto_yaw_rate_limit();
-
 	AttitudeControl _attitude_control; /**< class for attitude control calculations */
 
 	int		_v_att_sub{-1};			/**< vehicle attitude subscription */
@@ -249,7 +246,6 @@ private:
 		(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
 		(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
 		(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
-		(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _param_mc_yawrauto_max,
 		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,			/**< scaling factor from stick to yaw rate */
 
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -149,7 +149,6 @@ MulticopterAttitudeControl::parameters_updated()
 	// angular rate limits
 	using math::radians;
 	_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get())));
-	adapt_auto_yaw_rate_limit();
 
 	// manual rate control acro mode rate limits
 	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get()));
@@ -195,16 +194,6 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
 
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-		adapt_auto_yaw_rate_limit();
-	}
-}
-
-void MulticopterAttitudeControl::adapt_auto_yaw_rate_limit() {
-	if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
-		!_v_control_mode.flag_control_manual_enabled) {
-		_attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrauto_max.get()));
-	} else {
-		_attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrate_max.get()));
 	}
 }