Smooth yaw by limiting acceleration

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

@@ -70,7 +70,8 @@ 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;
+	const float heading = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
+	_heading_smoothing.reset(heading, 0.f);
 	_updateTrajConstraints();
 	_is_emergency_braking_active = false;
 	_time_last_cruise_speed_override = 0;
@@ -190,16 +191,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;
 		}
 	}
 
 	// 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 +256,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;
 		}
 
 		rcHelpModifyYaw(_land_heading);
@@ -300,39 +299,25 @@ 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());
 
 	_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)) {
+		_heading_smoothing.update(_yaw_setpoint, _deltatime);
+		float yaw_sp_prev = _yaw_setpoint;
+		_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;
-
-		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();
-		}
+		_yaw_sp_aligned = fabsf(matrix::wrap_pi(yaw_sp_prev - _yaw_setpoint)) < math::radians(_param_mis_yaw_err.get());
 	}
 
-	_yaw_sp_prev = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
-
 	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 +710,7 @@ void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz)
 
 void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
 {
-	_yaw_sp_prev += delta_psi;
+	_yaw_setpoint += delta_psi;
 }
 
 void FlightTaskAuto::_checkEmergencyBraking()
@@ -835,6 +820,12 @@ void FlightTaskAuto::_updateTrajConstraints()
 	// correction required by the altitude/vertical position controller
 	_constraints.speed_down = math::max(_constraints.speed_down, 1.2f * _param_mpc_z_v_auto_dn.get());;
 	_constraints.speed_up = math::max(_constraints.speed_up, 1.2f * _param_mpc_z_v_auto_up.get());;
+
+	// Update constrains of heading smoothing
+	const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
+	const float yawrate_acc = math::radians(_param_mpc_yawrauto_acc.get());
+	_heading_smoothing.setMaxHeadingRate(yawrate_max);
+	_heading_smoothing.setMaxHeadingAccel(yawrate_acc);
 }
 
 bool FlightTaskAuto::_highEnoughForLandingGear()

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

@@ -52,6 +52,7 @@
 #include <lib/weather_vane/WeatherVane.hpp>
 #include "Sticks.hpp"
 #include "StickAccelerationXY.hpp"
+#include "HeadingSmoothing.hpp"
 
 /**
  * This enum has to agree with position_setpoint_s type definition
@@ -136,8 +137,7 @@ protected:
 	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;
+	HeadingSmoothing _heading_smoothing;
 	bool _yaw_sp_aligned{false};
 
 	PositionSmoothing _position_smoothing;
@@ -156,6 +156,7 @@ 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,
+					(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 +202,7 @@ private:
 
 	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. */