diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 68e5b0a0f8..a6dd0e6fbb 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -45,5 +45,6 @@ px4_add_module( launchdetection landing_slope runway_takeoff + SlewRate tecs ) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index cef3ce24ab..4119ecde37 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -72,6 +72,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : _tecs_status_pub.advertise(); + _slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE); + /* fetch initial parameter values */ parameters_update(); } @@ -401,17 +403,15 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get()); // initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case - const bool outside_of_limits = _last_airspeed_setpoint < airspeed_min_adjusted - || _last_airspeed_setpoint > _param_fw_airspd_max.get(); + const bool outside_of_limits = _slew_rate_airspeed.getState() < airspeed_min_adjusted + || _slew_rate_airspeed.getState() > _param_fw_airspd_max.get(); - if (!PX4_ISFINITE(_last_airspeed_setpoint) || outside_of_limits) { - _last_airspeed_setpoint = airspeed_setpoint; + if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) { + _slew_rate_airspeed.setForcedValue(airspeed_setpoint); } else if (dt > FLT_EPSILON) { // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s - airspeed_setpoint = constrain(airspeed_setpoint, _last_airspeed_setpoint - ASPD_SP_SLEW_RATE * dt, - _last_airspeed_setpoint + ASPD_SP_SLEW_RATE * dt); - _last_airspeed_setpoint = airspeed_setpoint; + airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, dt); } return airspeed_setpoint; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 5d0c64ddec..27c104258e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -254,8 +255,6 @@ private: float _manual_control_setpoint_altitude{0.0f}; float _manual_control_setpoint_airspeed{0.0f}; - float _last_airspeed_setpoint{0.f}; - hrt_abstime _time_in_fixed_bank_loiter{0}; ECL_L1_Pos_Controller _l1_control; @@ -364,6 +363,8 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); + SlewRate _slew_rate_airspeed; + /* * Call TECS : a wrapper function to call the TECS implementation */