dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW Pos Control: use SlewRate library for airspeed setpoint 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-01-06 18:29:57 +01:00 committed by Daniel Agar
parent 30ccfdb2ed
commit 5d7ddf5734
3 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/fw_pos_control_l1/CMakeLists.txt
View File

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

14
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

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

5
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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
*/