forked from Archive/PX4-Autopilot
FW Pos Control: use SlewRate library for airspeed setpoint
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
30ccfdb2ed
commit
5d7ddf5734
|
@ -45,5 +45,6 @@ px4_add_module(
|
|||
launchdetection
|
||||
landing_slope
|
||||
runway_takeoff
|
||||
SlewRate
|
||||
tecs
|
||||
)
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
Loading…
Reference in New Issue