diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..6ab03e46334 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3524,7 +3524,7 @@ 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. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable. +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 | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c16ce93b21..36646efb2c0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -83,7 +83,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"] - name: aux_operator values: ["OR", "AND"] @@ -2545,7 +2545,7 @@ 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. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable." + 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 min: 0 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 71a7f99fc34..ff38b2e17e5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -399,38 +399,52 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ - if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // courseVirtualCorrection initially used to determine current position relative to course line for later use - int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); - navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); - - // tracking only active when certain distance and heading conditions are met - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) { - int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog); - - // captureFactor adjusts distance/heading sensitivity balance when closing in on course line. - // Closing distance threashold based on speed and an assumed 1 second response time. - float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f; - - // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy - // initial courseCorrectionFactor based on distance to course line - float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f); - courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor; - - // course heading alignment factor - float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f); - courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; - - // final courseCorrectionFactor combining distance and heading factors - courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); - - // final courseVirtualCorrection value - courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; - virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection); + if (isWaypointNavTrackingActive()) { + /* Calculate cross track error */ + posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); + + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) { + static float crossTrackErrorRate; + static timeUs_t previousCrossTrackErrorUpdateTime; + static float previousCrossTrackError = 0.0f; + static pt1Filter_t fwCrossTrackErrorRateFilterState; + + 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; + } + + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); + + 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 */ + float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); + float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); + adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); + + /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); + virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); + } } } - /* * Calculate NAV heading error * Units are centidegrees