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

fw pos ctrl: absorb fw_lnd_rel_ter into fw_lnd_useter parameter

This commit is contained in:
Thomas Stastny 2022-07-18 18:39:55 +02:00 committed by Daniel Agar
parent de3ac12ecd
commit 413ce8a3c4
3 changed files with 23 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1686,13 +1686,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
_landing_approach_entrance_rel_alt);
const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);
float altitude_setpoint;
const float glide_slope_reference_alt = (_param_fw_lnd_useter.get() ==
TerrainEstimateUseOnLanding::kFollowTerrainRelativeLandingGlideSlope) ? terrain_alt : pos_sp_curr.alt;
// by default the landing waypoint altitude is used for the glide slope reference. this ensures a constant slope
// during the landing approach, despite any terrain bumps (think tall trees below the landing approach) disrupting
// the glide behavior. in this case, when FW_LND_USETER==1, the terrain estimate is only used to trigger the flare.
// however - the option still exists to make the glide slope terrain relative via FW_LND_TER_REL.
const float glide_slope_reference_alt = (_param_fw_lnd_ter_rel.get()) ? terrain_alt : pos_sp_curr.alt;
float altitude_setpoint;
if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) {
// descend to the glide slope
@ -1706,6 +1703,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get());
// the terrain estimate (if enabled) is always used to determine the flaring altitude
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) {
// flare and land with minimal speed
@ -2655,7 +2653,7 @@ FixedwingPositionControl::calculateLandingApproachVector() const
float
FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude)
{
if (_param_fw_lnd_useter.get()) {
if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) {
if (_local_pos.dist_bottom_valid) {

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

@ -350,6 +350,12 @@ private:
// [us] time at which we had last valid terrain alt
hrt_abstime _last_time_terrain_alt_was_valid{0};
enum TerrainEstimateUseOnLanding {
kDisableTerrainEstimation = 0,
kTriggerFlareWithTerrainEstimate,
kFollowTerrainRelativeLandingGlideSlope
};
// AIRSPEED
float _airspeed{0.0f};
@ -794,7 +800,7 @@ private:
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
(ParamBool<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
@ -856,8 +862,7 @@ private:
(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
(ParamBool<px4::params::FW_LND_TER_REL>) _param_fw_lnd_ter_rel
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort
)
};

28
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -411,14 +411,20 @@ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f);
/**
* Use terrain estimate during landing. This is critical for detecting when to flare, and should be enabled if possible.
* Use terrain estimation during landing. This is critical for detecting when to flare, and should be enabled if possible.
*
* If enabled and no measurement is found within a given timeout, the landing waypoint will be used or the
* landing will be aborted, depending on the criteria set in FW_LND_ABORT.
* NOTE: terrain estimate is currently solely derived from a distance sensor.
*
* If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing
* will be aborted, depending on the criteria set in FW_LND_ABORT.
*
* If disabled, FW_LND_ABORT terrain based criteria are ignored.
*
* @boolean
* @min 0
* @max 2
* @value 0 Disable the terrain estimate
* @value 1 Use the terrain estimate to trigger the flare (only)
* @value 2 Calculate landing glide slope relative to the terrain estimate
* @group FW L1 Control
*/
PARAM_DEFINE_INT32(FW_LND_USETER, 1);
@ -1070,17 +1076,3 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2);
* @group FW L1 Control
*/
PARAM_DEFINE_INT32(FW_LND_ABORT, 3);
/**
* Calculate the landing glide slope relative to the terrain estimate.
*
* If enabled, the terrain estimate (e.g. via distance sensor) will be used as the glide slope reference altitude, following
* all bumps in the terrain below the landing approach.
*
* If disabled, the land waypoint altitude will be a fixed glide slope reference, and the distance sensor (if enabled via
* FW_LND_USETER) will only be used to trigger the flare.
*
* @boolean
* @group FW L1 Control
*/
PARAM_DEFINE_INT32(FW_LND_TER_REL, 0);