diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index ab9999b185..1b26f7998d 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -47,4 +47,5 @@ px4_add_module( controllib git_ecl ecl_geo + SlewRate ) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d219c189ed..b46f5b539b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -49,12 +49,9 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) : _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD") { - // fetch initial parameter values parameters_update(true); - - // set failsafe hysteresis _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); - + _tilt_limit_slew_rate.setSlewRate(.2f); reset_setpoint_to_nan(_setpoint); } @@ -378,13 +375,14 @@ void MulticopterPositionControl::Run() } // limit tilt during takeoff ramupup - if (_takeoff.getTakeoffState() < TakeoffState::flight) { - _control.setTiltLimit(math::radians(_param_mpc_tiltmax_lnd.get())); + float tilt_limit = math::radians(_param_mpc_tiltmax_air.get()); - } else { - _control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get())); + if (_takeoff.getTakeoffState() < TakeoffState::flight) { + tilt_limit = math::radians(_param_mpc_tiltmax_lnd.get()); } + _control.setTiltLimit(_tilt_limit_slew_rate.update(tilt_limit, dt)); + const float speed_up = _takeoff.updateRamp(dt, PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 2141686446..eff575bcb1 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -197,6 +198,7 @@ private: static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ + SlewRate _tilt_limit_slew_rate; uint8_t _old_takeoff_state{};