mc_att_control: add an optional stick tilt input filter

src/modules/mc_att_control/mc_att_control.hpp

@@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <vtol_att_control/vtol_type.h>
+#include <lib/ecl/EKF/AlphaFilter.hpp>
 
 #include <AttitudeControl.hpp>
 
@@ -130,6 +131,9 @@ private:
 	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
 
 	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
+	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
+	AlphaFilter<float> _man_x_input_filter;
+	AlphaFilter<float> _man_y_input_filter;
 
 	hrt_abstime _last_run{0};
 
@@ -157,12 +161,10 @@ private:
 		_param_mpc_thr_hover,			/**< throttle at which vehicle is at hover equilibrium */
 		(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve,				/**< throttle curve behavior */
 
-		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
+		(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
 	)
 
 	bool _is_tailsitter{false};
-
-	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
-
 };
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -154,8 +154,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	 * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
 	 * points to, and changes of the stick input are linear.
 	 */
-	const float x = _manual_control_sp.x * _man_tilt_max;
-	const float y = _manual_control_sp.y * _man_tilt_max;
+	_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
+	_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
+	_man_x_input_filter.update(_manual_control_sp.x * _man_tilt_max);
+	_man_y_input_filter.update(_manual_control_sp.y * _man_tilt_max);
+	const float x = _man_x_input_filter.getState();
+	const float y = _man_y_input_filter.getState();
 
 	// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane
 	Vector2f v = Vector2f(y, -x);
@@ -322,6 +326,10 @@ MulticopterAttitudeControl::Run()
 
 				generate_attitude_setpoint(dt, _reset_yaw_sp);
 				attitude_setpoint_generated = true;
+
+			} else {
+				_man_x_input_filter.reset(0.f);
+				_man_y_input_filter.reset(0.f);
 			}
 
 			control_attitude();

src/modules/mc_att_control/mc_att_control_params.c

@@ -163,3 +163,15 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f);
+
+/**
+ * Manual tilt input filter time constant
+ * Setting this parameter to 0 disables the filter
+ *
+ * @unit s
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f);