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

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