StickYaw: use consistently for all flight tasks

- Switching to the first order filter that was previously
only in FlightTaskManualAltitude.
- Moving the scaling of full stick deflection to
radians per second into the class.

src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -243,9 +243,9 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		vertical_speed *= (1 + _sticks.getPositionExpo()(2));
 
 		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-		if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
+		if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
-			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-						       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
+						       _deltatime);
 		}
 
 		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -142,7 +142,7 @@ protected:
 	Vector3f _unsmoothed_velocity_setpoint;
 	Sticks _sticks{this};
 	StickAccelerationXY _stick_acceleration_xy{this};
-	StickYaw _stick_yaw;
+	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
@@ -176,8 +176,7 @@ protected:
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
 					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
-					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
+					_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
 				       );
 
 private:

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -63,8 +63,8 @@ bool FlightTaskManualAcceleration::update()
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
+				       _deltatime);
 
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint,
 			_position,

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
 
 	StickAccelerationXY _stick_acceleration_xy{this};
-	StickYaw _stick_yaw;
+	StickYaw _stick_yaw{this};
 
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,10 +42,6 @@
 
 using namespace matrix;
 
-FlightTaskManualAltitude::FlightTaskManualAltitude() :
-	_sticks(this)
-{}
-
 bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
@@ -95,8 +91,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
-	const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
-	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
+				       _is_yaw_good_for_control, _deltatime);
 
 	// Use sticks input with deadzone and exponential curve for vertical velocity
 	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() :
@@ -104,14 +100,6 @@ void FlightTaskManualAltitude::_scaleSticks()
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
 
-float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
-{
-	const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
-	const float alpha = _deltatime / den;
-	_yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
-	return _yawspeed_filter_state;
-}
-
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
 	// Depending on stick inputs and velocity, position is locked.

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,14 +40,15 @@
 #pragma once
 
 #include "FlightTask.hpp"
-#include "Sticks.hpp"
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include "Sticks.hpp"
+#include "StickYaw.hpp"
 #include <uORB/Subscription.hpp>
 
 class FlightTaskManualAltitude : public FlightTask
 {
 public:
-	FlightTaskManualAltitude();
+	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
@@ -73,7 +74,8 @@ protected:
 	 */
 	void _updateAltitudeLock();
 
-	Sticks _sticks;
+	Sticks _sticks{this};
+	StickYaw _stick_yaw{this};
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
@@ -81,8 +83,6 @@ protected:
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
-					(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
 					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
@@ -97,14 +97,6 @@ private:
 	void _unlockYaw();
 	void _lockYaw();
 
-	/**
-	 * Filter between stick input and yaw setpoint
-	 *
-	 * @param yawspeed_target yaw setpoint desired by the stick
-	 * @return filtered value from independent filter state
-	 */
-	float _applyYawspeedFilter(float yawspeed_target);
-
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -132,7 +124,6 @@ private:
 
 	void setGearAccordingToSwitch();
 
-	float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
 	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */

src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt

@@ -38,5 +38,5 @@ px4_add_library(FlightTaskUtility
 	StickYaw.cpp
 )
 
-target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning)
+target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)
 target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,23 +31,19 @@
  *
  ****************************************************************************/
 
-/**
- * @file StickYaw.cpp
- */
-
 #include "StickYaw.hpp"
 
 #include <px4_platform_common/defines.h>
 
-StickYaw::StickYaw()
+StickYaw::StickYaw(ModuleParams *parent) :
-{
+	ModuleParams(parent)
-	_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
+{}
-}
 
-void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
+void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw,
 				   const float yaw, const bool is_yaw_good_for_control, const float deltatime)
 {
-	yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
+	_yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get());
+	yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get()));
 	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control);
 }
 

src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,19 +39,20 @@
 
 #pragma once
 
-#include "SlewRate.hpp"
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <px4_platform_common/module_params.h>
 
-class StickYaw
+class StickYaw : public ModuleParams
 {
 public:
-	StickYaw();
+	StickYaw(ModuleParams *parent);
 	~StickYaw() = default;
 
-	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float desired_yawspeed, float yaw,
+	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw,
 				 bool is_yaw_good_for_control, float deltatime);
 
 private:
-	SlewRate<float> _yawspeed_slew_rate;
+	AlphaFilter<float> _yawspeed_filter;
 
 	/**
 	 * Lock yaw when not currently turning
@@ -65,4 +66,9 @@ private:
 	 * @return yaw setpoint to execute to have a yaw lock at the correct moment in time
 	 */
 	static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control);
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection
+		(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau ///< time constant for yaw speed filtering
+	)
 };