FW Pos Control: use SlewRate library for airspeed setpoint

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -45,5 +45,6 @@ px4_add_module(
 		launchdetection
 		landing_slope
 		runway_takeoff
+		SlewRate
 		tecs
 	)

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;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -60,6 +60,7 @@
 #include <lib/landing_slope/Landingslope.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRate.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -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<float> _slew_rate_airspeed;
+
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
 	 */