Smooth yaw in auto mode by limiting acceleration

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -70,7 +70,10 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 
 	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
 
-	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
+	_yaw_setpoint_previous = last_setpoint.yaw;
+	_heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw,
+				 PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f);
+
 	_updateTrajConstraints();
 	_is_emergency_braking_active = false;
 	_time_last_cruise_speed_override = 0;
@@ -190,16 +193,14 @@ bool FlightTaskAuto::update()
 		// no valid heading -> generate heading in this flight task
 		// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
 		if (!_generateHeadingAlongTraj()) {
-			_yaw_setpoint = PX4_ISFINITE(_yaw_sp_prev) ? _yaw_sp_prev : _yaw;
+			_yaw_setpoint = _yaw_setpoint_previous;
 		}
 	}
 
 	// update previous type
 	_type_previous = _type;
 
-	// If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
-	// it will see its setpoint constrained here
-	_limitYawRate();
+	_smoothYaw();
 
 	_constraints.want_takeoff = _checkTakeoff();
 
@@ -257,7 +258,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 
 		if (sticks_xy.longerThan(FLT_EPSILON)) {
 			// Ensure no unintended yawing when nudging horizontally during initial heading alignment
-			_land_heading = _yaw_sp_prev;
+			_land_heading = _yaw_setpoint_previous;
 		}
 
 		rcHelpModifyYaw(_land_heading);
@@ -300,39 +301,32 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 
-void FlightTaskAuto::_limitYawRate()
+void FlightTaskAuto::_smoothYaw()
 {
 	const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
+	_heading_smoothing.setMaxHeadingRate(yawrate_max);
+	_heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get()));
 
 	_yaw_sp_aligned = true;
 
-	if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) {
-		// Limit the rate of change of the yaw setpoint
-		const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
-		const float dyaw_max = yawrate_max * _deltatime;
-		const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
-		const float yaw_setpoint_sat = matrix::wrap_pi(_yaw_sp_prev + dyaw);
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		const float yaw_sp_unsmoothed = _yaw_setpoint;
+		_heading_smoothing.update(_yaw_setpoint, _deltatime);
+		_yaw_setpoint = _heading_smoothing.getSmoothedHeading();
+		_yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate();
 
 		// The yaw setpoint is aligned when it is within tolerance
-		_yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get());
-
-		_yaw_setpoint = yaw_setpoint_sat;
+		_yaw_sp_aligned = fabsf(matrix::wrap_pi(yaw_sp_unsmoothed - _yaw_setpoint)) < math::radians(_param_mis_yaw_err.get());
 
-		if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
-			// Create a feedforward using the filtered derivative
-			_yawspeed_filter.setParameters(_deltatime, .2f);
-			_yawspeed_filter.update(dyaw / _deltatime);
-			_yawspeed_setpoint = _yawspeed_filter.getState();
-		}
+	} else {
+		_heading_smoothing.reset(_yaw, 0.f);
 	}
 
-	_yaw_sp_prev = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
+	_yaw_setpoint_previous = _yaw_setpoint;
 
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {
 		// The yaw setpoint is aligned when its rate is not saturated
 		_yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max);
-
-		_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
 	}
 }
 
@@ -725,7 +719,7 @@ void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz)
 
 void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
 {
-	_yaw_sp_prev += delta_psi;
+	_yaw_setpoint_previous += delta_psi;
 }
 
 void FlightTaskAuto::_checkEmergencyBraking()

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -47,6 +47,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/motion_planning/HeadingSmoothing.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/stick_yaw/StickYaw.hpp>
 #include <lib/weather_vane/WeatherVane.hpp>
@@ -136,8 +137,8 @@ class FlightTaskAuto : public FlightTask
 	State _current_state{State::none};
 	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
 
-	float _yaw_sp_prev{NAN};
-	AlphaFilter<float> _yawspeed_filter;
+	float _yaw_setpoint_previous{NAN}; /**< Used because _yaw_setpoint is overwritten in multiple places */
+	HeadingSmoothing _heading_smoothing;
 	bool _yaw_sp_aligned{false};
 
 	PositionSmoothing _position_smoothing;
@@ -156,6 +157,7 @@ class FlightTaskAuto : public FlightTask
 					(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,
+					(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,
 					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
 					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
@@ -201,7 +203,7 @@ class FlightTaskAuto : public FlightTask
 
 	matrix::Vector3f _initial_land_position;
 
-	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
+	void _smoothYaw(); /**< Smoothen 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/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -179,8 +179,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 	_orbit_velocity = 1.f;
 	_center = _position;
 	_initial_heading = _yaw;
-	_slew_rate_yaw.setForcedValue(_yaw);
-	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw,
+				 PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f);
 	_slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get());
 
 	// need a valid position and velocity
@@ -238,7 +238,14 @@ bool FlightTaskOrbit::update()
 	}
 
 	// Apply yaw smoothing
-	_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
+	_heading_smoothing.setMaxHeadingRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get()));
+	_heading_smoothing.update(_yaw_setpoint, _deltatime);
+	_yaw_setpoint = _heading_smoothing.getSmoothedHeading();
+
+	if (_in_circle_approach) { // don't override feed-forward which is already calculated for circling
+		_yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate();
+	}
 
 	// publish information to UI
 	sendTelemetry();
@@ -362,7 +369,7 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints()
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
 		// no yaw setpoint
 		_yaw_setpoint = NAN;
-		_yawspeed_setpoint = NAN;
+		_yawspeed_setpoint = 0.f; // No yaw setpoint is invalid -> just brake to 0°/s
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -45,6 +45,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/orbit_status.h>
 #include <lib/slew_rate/SlewRateYaw.hpp>
+#include <lib/motion_planning/HeadingSmoothing.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/motion_planning/VelocitySmoothing.hpp>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -127,7 +128,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 	bool _started_clockwise{true};
 	bool _currently_orbiting{false};
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
-	SlewRateYaw<float> _slew_rate_yaw;
+	HeadingSmoothing _heading_smoothing;
 	SlewRate<float> _slew_rate_velocity;
 
 	orb_advert_t _mavlink_log_pub{nullptr};
@@ -138,6 +139,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 		(ParamInt<px4::params::MC_ORBIT_YAW_MOD>) _param_mc_orbit_yaw_mod,
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
 		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
+		(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,
 		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
 		(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
 		_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated

src/modules/mc_pos_control/multicopter_autonomous_params.c

@@ -145,7 +145,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f);
  * @increment 5
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f);
+PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 60.f);
 
 /**
  * Maximum yaw acceleration in autonomous modes
@@ -160,7 +160,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f);
  * @increment 5
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f);
+PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 20.f);
 
 /**
  * Heading behavior in autonomous modes