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/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()
-{
-	_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
-}
+StickYaw::StickYaw(ModuleParams *parent) :
+	ModuleParams(parent)
+{}
 
-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
+	)
 };