diff --git a/docs/Settings.md b/docs/Settings.md index c5965d13361..6ab03e46334 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3524,11 +3524,11 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_wp_tracking_accuracy -Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. +Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 0 | 0 | 10 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3aa1f481fc..36646efb2c0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2545,10 +2545,11 @@ groups: min: 1 max: 9 - name: nav_fw_wp_tracking_accuracy - description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible." - default_value: OFF + description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy." + default_value: 0 field: fw.wp_tracking_accuracy - type: bool + min: 0 + max: 10 - name: nav_fw_wp_tracking_max_angle description: "Sets the maximum allowed alignment convergence angle to the waypoint course line when nav_fw_wp_tracking_accuracy is active [degrees]. Lower values result in smoother alignment with the course line but will take more distance until this is achieved." default_value: 60 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4d8215c688d..e09fbea57a9 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -399,7 +399,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - // Calculate cross track error + /* Calculate cross track error */ fpVector3_t virtualCoursePoint; virtualCoursePoint.x = posControl.activeWaypoint.pos.x - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); @@ -407,8 +407,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - /* If waypoint tracking enabled force craft toward and ckosely track along waypoint course line */ - if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ + if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { static float crossTrackErrorRate; static timeUs_t previousCrossTrackErrorUpdateTime; static float previousCrossTrackError = 0.0f; @@ -416,43 +416,28 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; } crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); - previousCrossTrackErrorUpdateTime = currentTimeUs; previousCrossTrackError = navCrossTrackError; } - /* Dynamic tracking deadband set at minimum 2m for baseline speed of 60 km/h */ - float trackingDeadband = 200.0f * MAX(posControl.actualState.velXY / 1700.0f, 1.0f); - - /* cutTurnActive improves convergence with course line when WP turn smoothing CUT used */ - static bool cutTurnActive = false; - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { - cutTurnActive = true; - } + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); - if (cutTurnActive && !posControl.flags.wpTurnSmoothingActive) { - virtualTargetBearing = wrap_36000(virtualTargetBearing - wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing)); - cutTurnActive = ABS(wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog)) > 500; - } else if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && - navCrossTrackError > trackingDeadband) { + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); - /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile - * Adjustment limited to navCrossTrackError as course line approached to avoid instability */ + /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); - float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); - float limit = MIN(angleLimit, navCrossTrackError); - + float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed); adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); - adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } }