From cba09f618868792e332f6db982f7b4c5dae19055 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Fri, 10 Apr 2026 18:11:10 +0200 Subject: [PATCH] Offset FW autoland loiter position perpendicular to runway Shift the wind-check loiter circle away from the landing point by loiter_radius in the configured approach direction (LEFT/RIGHT), perpendicular to the runway heading. This places the loiter pattern on the same side as the base leg turn point, reducing the distance to the first approach waypoint after wind assessment. --- src/main/navigation/navigation.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 03d4a3da36b..d19d8bea249 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2462,7 +2462,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig } } - fpVector3_t tmpPoint = posControl.fwLandState.landPos; + // Offset loiter position perpendicular to runway in the approach circuit direction (LEFT/RIGHT) + fpVector3_t tmpPoint; + int32_t refHeading = 0; + if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1 != 0) { + refHeading = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1)); + } else if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2 != 0) { + refHeading = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2)); + } + + if (refHeading != 0) { + int32_t offsetDir; + if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) { + offsetDir = wrap_36000(refHeading - 9000); + } else { + offsetDir = wrap_36000(refHeading + 9000); + } + calculateFarAwayPos(&tmpPoint, &posControl.fwLandState.landPos, offsetDir, navConfig()->fw.loiter_radius); + } else { + tmpPoint = posControl.fwLandState.landPos; + } + tmpPoint.z = posControl.fwLandState.landAproachAltAgl; setDesiredPosition(&tmpPoint, posControl.fwLandState.landPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);