From d1b8a2e8d5b2c841941272634109186fcbb3358a Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 29 Nov 2023 17:16:45 +0100 Subject: [PATCH] fxedwingPositionControl: Add slew rate at the end for all mode instead inside each --- src/lib/npfg/npfg.cpp | 5 ---- src/lib/npfg/npfg.hpp | 16 ------------ .../FixedwingPositionControl.cpp | 25 ++++++++----------- .../FixedwingPositionControl.hpp | 2 ++ 4 files changed, 13 insertions(+), 35 deletions(-) diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index a3d9b703f6..fdcfa570b0 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -502,11 +502,6 @@ void NPFG::updateRollSetpoint() float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G); roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_); - if (dt_ > 0.0f && roll_slew_rate_ > 0.0f) { - // slew rate limiting active - roll_new = math::constrain(roll_new, roll_setpoint_ - roll_slew_rate_ * dt_, roll_setpoint_ + roll_slew_rate_ * dt_); - } - if (PX4_ISFINITE(roll_new)) { roll_setpoint_ = roll_new; } diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index 1d185d48ed..5afedf8907 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -247,20 +247,6 @@ public: */ void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; } - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Set roll angle slew rate. Set to zero to deactivate. - */ - void setRollSlewRate(float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; } - - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting. - */ - void setDt(const float dt) { dt_ = dt; } - /* * [Copied directly from ECL_L1_Pos_Controller] * @@ -364,10 +350,8 @@ private: * ECL_L1_Pos_Controller functionality */ - float dt_{0}; // control loop time [s] float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad] float roll_setpoint_{0.0f}; // current roll angle setpoint [rad] - float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s] /* * Adapts the controller period considering user defined inputs, current flight diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9739fb7604..8fb08925a0 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -81,6 +81,9 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : parameters_update(); _airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed()); + _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); + _roll_slew_rate.setForcedValue(0.f); + } FixedwingPositionControl::~FixedwingPositionControl() @@ -106,6 +109,8 @@ FixedwingPositionControl::parameters_update() _performance_model.updateParameters(); + _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); + // VTOL parameter VT_ARSP_TRANS if (_param_handle_airspeed_trans != PARAM_INVALID) { param_get(_param_handle_airspeed_trans, &_param_airspeed_trans); @@ -124,7 +129,6 @@ FixedwingPositionControl::parameters_update() _npfg.setRollTimeConst(_param_npfg_roll_time_const.get()); _npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); _npfg.setRollLimit(radians(_param_fw_r_lim.get())); - _npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max.get())); _npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); // TECS parameters @@ -2120,16 +2124,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - // do slew rate limiting on roll if enabled - float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - const float roll_rate_slew_rad = radians(_param_fw_pn_r_slew_max.get()); - - if (control_interval > 0.f && roll_rate_slew_rad > 0.f) { - roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval, - _att_sp.roll_body + roll_rate_slew_rad * control_interval); - } - - _att_sp.roll_body = roll_sp_new; + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } @@ -2349,9 +2344,6 @@ FixedwingPositionControl::Run() update_in_air_states(_local_pos.timestamp); - // update lateral guidance timesteps for slewrates - _npfg.setDt(control_interval); - // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); @@ -2451,6 +2443,9 @@ FixedwingPositionControl::Run() _control_mode.flag_control_altitude_enabled || _control_mode.flag_control_climb_rate_enabled) { + // roll slew rate + _att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval); + const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -2465,6 +2460,8 @@ FixedwingPositionControl::Run() } } + } else { + _roll_slew_rate.setForcedValue(_roll); } // if there's any change in landing gear setpoint publish it diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index c2cb806b6d..da96aef75a 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -256,6 +256,7 @@ private: double _current_longitude{0}; float _current_altitude{0.f}; + float _roll{0.f}; float _pitch{0.0f}; float _yaw{0.0f}; float _yawrate{0.0f}; @@ -720,6 +721,7 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); SlewRate _airspeed_slew_rate_controller; + SlewRate _roll_slew_rate; /** * @brief A wrapper function to call the TECS implementation