fw pos ctrl: enumerate landing nudging options, add to param documenation

This commit is contained in:
Thomas Stastny 2022-07-13 11:25:43 +02:00 committed by Daniel Agar
parent 9fab914687
commit c98153e044
3 changed files with 18 additions and 6 deletions

View File

@ -2566,7 +2566,8 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
Vector2f
FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
{
if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE && _param_fw_lnd_nudge.get() > 0) {
if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
&& _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) {
// laterally nudge touchdown location with yaw stick
// positive is defined in the direction of a right hand turn starting from the approach vector direction
const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
@ -2590,17 +2591,17 @@ FixedwingPositionControl::calculateLandingApproachVector() const
const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero();
const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};
// if _param_fw_lnd_nudge.get() == 0, no nudging
if (_param_fw_lnd_nudge.get() == 1) {
if (_param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachAngle) {
// nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach
// entrance position to the newly nudged touchdown point
// NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly
landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset;
}
// if _param_fw_lnd_nudge.get() == 2, the full path (including approach entrance point) is nudged with the touchdown
// point, which does not require any additions to the approach vector
// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgingDisabled, no nudging
// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachPath, the full path (including approach
// entrance point) is nudged with the touchdown point, which does not require any additions to the approach vector
return landing_approach_vector;
}

View File

@ -316,6 +316,13 @@ private:
// AUTO LANDING
// corresponds to param FW_LND_NUDGE
enum LandingNudgingOption {
kNudgingDisabled = 0,
kNudgeApproachAngle,
kNudgeApproachPath
};
hrt_abstime _time_started_landing{0}; // [us]
// [m] lateral touchdown position offset manually commanded during landing

View File

@ -1032,6 +1032,10 @@ PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0);
* Approach angle nudging: shifts the touchdown point laterally while keeping the approach entrance point constant
* Approach path nudging: shifts the touchdown point laterally along with the entire approach path
*
* This is useful for manually adjusting the landing point in real time when map or GNSS errors cause an offset from the
* desired landing vector. Nuding is done with yaw stick, constrained to FW_LND_TD_OFF (in meters) and the direction is
* relative to the vehicle heading (stick deflection to the right = land point moves to the right as seen by the vehicle).
*
* @min 0
* @max 2
* @value 0 Disable nudging