fxedwingPositionControl: Add slew rate at the end for all mode instead inside each

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;
 	}

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

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

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<float> _airspeed_slew_rate_controller;
+	SlewRate<float> _roll_slew_rate;
 
 	/**
 	 * @brief A wrapper function to call the TECS implementation