diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 708c8b7437..84e6f1ecb1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 75fde26a00..82780799db 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index d33529258d..9c845e29c1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -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