From 084858028c692e287bee8c3e21cefc291aa658cd Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 20 Aug 2024 17:41:14 +0100 Subject: [PATCH] cr132 --- docs/Settings.md | 4 +-- src/main/fc/settings.yaml | 7 ++-- src/main/navigation/navigation.c | 6 ++-- src/main/navigation/navigation_fixedwing.c | 37 +++++----------------- 4 files changed, 17 insertions(+), 37 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 09b25f87b05..1196ffee73f 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 026733272f5..853d58471fa 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2556,10 +2556,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 # // CR132 - 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." diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1b3963e5e15..b6ac50044ce 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4766,9 +4766,9 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 50.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f, // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // poss new FF, original was 0.0f 0.0f, NAV_DTERM_CUT_HZ, diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index a6951a83330..0f1c7b6691a 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -433,47 +433,26 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing); DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate); - /* 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); + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); - /* 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; - } - DEBUG_SET(DEBUG_ALWAYS, 5, cutTurnActive); - 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); + DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed); + DEBUG_SET(DEBUG_ALWAYS, 5, adjustmentFactor); /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); - // old code - // float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); - // // float limit = constrainf(fabsf(crossTrackErrorRate) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); - // float rateFactor = limit; - // if (crossTrackErrorRate > 0.0f) { - // float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f); - // rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit); - // } - DEBUG_SET(DEBUG_ALWAYS, 7, limit); DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor); - DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed); } DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing); }