mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
fw pos ctrl: absorb fw_lnd_rel_ter into fw_lnd_useter parameter
This commit is contained in:
committed by
Daniel Agar
parent
de3ac12ecd
commit
413ce8a3c4
@@ -1686,13 +1686,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
|||||||
_landing_approach_entrance_rel_alt);
|
_landing_approach_entrance_rel_alt);
|
||||||
|
|
||||||
const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.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
|
float altitude_setpoint;
|
||||||
// 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;
|
|
||||||
|
|
||||||
if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) {
|
if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) {
|
||||||
// descend to the glide slope
|
// 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
|
// 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());
|
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) {
|
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) {
|
||||||
// flare and land with minimal speed
|
// flare and land with minimal speed
|
||||||
|
|
||||||
@@ -2655,7 +2653,7 @@ FixedwingPositionControl::calculateLandingApproachVector() const
|
|||||||
float
|
float
|
||||||
FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude)
|
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) {
|
if (_local_pos.dist_bottom_valid) {
|
||||||
|
|
||||||
|
|||||||
@@ -350,6 +350,12 @@ private:
|
|||||||
// [us] time at which we had last valid terrain alt
|
// [us] time at which we had last valid terrain alt
|
||||||
hrt_abstime _last_time_terrain_alt_was_valid{0};
|
hrt_abstime _last_time_terrain_alt_was_valid{0};
|
||||||
|
|
||||||
|
enum TerrainEstimateUseOnLanding {
|
||||||
|
kDisableTerrainEstimation = 0,
|
||||||
|
kTriggerFlareWithTerrainEstimate,
|
||||||
|
kFollowTerrainRelativeLandingGlideSlope
|
||||||
|
};
|
||||||
|
|
||||||
// AIRSPEED
|
// AIRSPEED
|
||||||
|
|
||||||
float _airspeed{0.0f};
|
float _airspeed{0.0f};
|
||||||
@@ -794,7 +800,7 @@ private:
|
|||||||
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
|
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
|
||||||
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
|
(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_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_MAX>) _param_fw_p_lim_max,
|
||||||
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
|
(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_FL_SINK>) _param_fw_lnd_fl_sink,
|
||||||
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
|
(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_NUDGE>) _param_fw_lnd_nudge,
|
||||||
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
|
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort
|
||||||
(ParamBool<px4::params::FW_LND_TER_REL>) _param_fw_lnd_ter_rel
|
|
||||||
)
|
)
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -411,14 +411,20 @@ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);
|
|||||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f);
|
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
|
* NOTE: terrain estimate is currently solely derived from a distance sensor.
|
||||||
* landing will be aborted, depending on the criteria set in FW_LND_ABORT.
|
*
|
||||||
|
* 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.
|
* 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
|
* @group FW L1 Control
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(FW_LND_USETER, 1);
|
PARAM_DEFINE_INT32(FW_LND_USETER, 1);
|
||||||
@@ -1070,17 +1076,3 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2);
|
|||||||
* @group FW L1 Control
|
* @group FW L1 Control
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(FW_LND_ABORT, 3);
|
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);
|
|
||||||
|
|||||||
Reference in New Issue
Block a user