diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 344b961679..24a8f19791 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -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 _man_x_input_filter; + AlphaFilter _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) _param_mpc_thr_curve, /**< throttle curve behavior */ - (ParamInt) _param_mc_airmode + (ParamInt) _param_mc_airmode, + (ParamFloat) _param_mc_man_tilt_tau ) bool _is_tailsitter{false}; - - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ - }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 591cdef1ef..4de41fa3b9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/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(); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index b616e6ac2f..9c4148c1e0 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/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);