From 047faca751913fb81f9cba35b6435f24f1793cd1 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 17:35:22 +0200 Subject: [PATCH 1/8] FW nav: share the turn-geometry primitives of the arc coordinator Move the bank-from-radius, bearing/unit-vector, circle-centre, line intersection, smoothstep and slew expressions that the turn coordinator repeated at up to ten sites into one extern TU, and reuse the planning radius in the loiter stabiliser instead of duplicating its formula. Expressions, operand order and lrintf placement are unchanged; a unit test compares every helper bit-exactly against the replaced inline code. MATEKF722 -384 B flash, MATEKH743 -896 B, RAM unchanged. Co-Authored-By: Claude Fable 5.1 --- src/main/CMakeLists.txt | 2 + src/main/navigation/navigation_fixedwing.c | 175 ++++---- .../navigation_fixedwing_turn_math.c | 116 ++++++ .../navigation_fixedwing_turn_math.h | 44 ++ src/test/unit/CMakeLists.txt | 3 + ...navigation_fixedwing_turn_math_unittest.cc | 379 ++++++++++++++++++ 6 files changed, 630 insertions(+), 89 deletions(-) create mode 100644 src/main/navigation/navigation_fixedwing_turn_math.c create mode 100644 src/main/navigation/navigation_fixedwing_turn_math.h create mode 100644 src/test/unit/navigation_fixedwing_turn_math_unittest.cc diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 054af703408..1cfd028c3f9 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -629,6 +629,8 @@ main_sources(COMMON_SRC navigation/navigation.c navigation/navigation.h navigation/navigation_fixedwing.c + navigation/navigation_fixedwing_turn_math.c + navigation/navigation_fixedwing_turn_math.h navigation/navigation_fw_launch.c navigation/navigation_geo.c navigation/navigation_multicopter.c diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 482f19b6950..96a3710af20 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -49,6 +49,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_fixedwing_autospeed_logic.h" +#include "navigation/navigation_fixedwing_turn_math.h" #include "navigation/navigation_private.h" #include "programming/logic_condition.h" @@ -450,8 +451,7 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, active = false; // window elapsed -> back to 1:1 } else { const float p = elapsedMs / tConstMs; - const float s = p * p * (3.0f - 2.0f * p); // smoothstep (S-curve) - out = rampStart + s * (rollTargetCd - rampStart); + out = fwSmoothBlend(rampStart, rollTargetCd, p); // smoothstep (S-curve) } } @@ -607,7 +607,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr float rollFF = 0.0f; if (ffRadius > 0.0f) { const float v = posControl.actualState.velXY; - const float phiFFcd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * ffRadius))); + const float phiFFcd = fwBankForRadiusCd(v, ffRadius); rollFF = ffSign * phiFFcd * (ffGain / 100.0f); } @@ -615,7 +615,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr // so the FF is a staircase. Rate limiting spreads the steps without lagging a settled command. const float maxStepCd = NAV_FW_FF_SLEW_FRACTION * (currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f) * 100.0f * US2S(deltaMicros); - fwTurnFFCmdCd += constrainf(rollFF - fwTurnFFCmdCd, -maxStepCd, maxStepCd); + fwTurnFFCmdCd = fwSlewToward(fwTurnFFCmdCd, rollFF, maxStepCd); DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(fwTurnFFCmdCd)); // turn/loiter roll feed-forward [centideg] return fwTurnFFCmdCd; @@ -632,9 +632,7 @@ static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearing static float netAngle = 0.0f; static float prevBearing = 0.0f; - const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); - const float required = constrainf((speed * speed) / (GRAVITY_CMSS * tan_approx(DEGREES_TO_RADIANS(getFwPlanningBankDeg()))), - NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); + const float required = getFwCoordinatedTurnRadius(); if (!loiterActive) { active = false; @@ -699,8 +697,7 @@ static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) return rollTargetCd; } - const float s = p * p * (3.0f - 2.0f * p); - return fwArcHandbackCmdCd + (rollTargetCd - fwArcHandbackCmdCd) * s; + return fwSmoothBlend(fwArcHandbackCmdCd, rollTargetCd, p); } // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive @@ -772,45 +769,41 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. const float px = posControl.activeWaypoint.pos.x; const float py = posControl.activeWaypoint.pos.y; - const int32_t brgToWp = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - pos->y, px - pos->x))))); + const int32_t brgToWp = fwRadToBearingCd(atan2_approx(py - pos->y, px - pos->x)); const int32_t toWpErr = wrap_18000(brgToWp - cog); if (ABS(toWpErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float phiTmp = fwBankForRadiusCd(v, arcRtmp); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); // Pin ahead by the roll-in drift so the ramp ends ON the circle - const float leadDist = 1.5f * v * (tTmp / 1000.0f); - const float cx = pos->x + leadDist * cos_approx(cogRad) + arcRtmp * cos_approx(cogRad + dirTmp * (M_PIf * 0.5f)); - const float cy = pos->y + leadDist * sin_approx(cogRad) + arcRtmp * sin_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + const float leadDist = fwRollInLeadCm(v, tTmp); + float leadX, leadY, cx, cy; + fwPolarOffset(pos->x, pos->y, leadDist, cogRad, &leadX, &leadY); + fwPerpOffset(leadX, leadY, arcRtmp, cogRad, dirTmp, &cx, &cy); if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { // Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the // leg, gamma = half the turn for shallow corners), short straight for the reverse // roll, then a standard corner-cut arc rolls out ON the line const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - const float ux = cos_approx(legRad), uy = sin_approx(legRad); + float ux, uy; + fwBearingUnit(legBearing, &ux, &uy); const int32_t turnCd = wrap_18000(legBearing - cog); const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(turnCd), 2000.0f, 4500.0f)); const float icptRad = legRad + (float)dirTmp * gammaRad; const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); - const float p1x = cx + 2.0f * arcRtmp * cos_approx(nAng); // intercept line shifted R toward the counter side - const float p1y = cy + 2.0f * arcRtmp * sin_approx(nAng); - const float lAng = legRad - (float)dirTmp * (M_PIf * 0.5f); - const float b0x = px + arcRtmp * cos_approx(lAng); - const float b0y = py + arcRtmp * sin_approx(lAng); - const float cross = d1x * uy - d1y * ux; - if (fabsf(cross) > 0.17f) { // gamma >= 20 deg keeps the lines well separated - const float tt = ((b0x - p1x) * uy - (b0y - p1y) * ux) / cross; - const float o2x = p1x + tt * d1x; - const float o2y = p1y + tt * d1y; + float p1x, p1y, b0x, b0y, o2x, o2y; + fwPolarOffset(cx, cy, 2.0f * arcRtmp, nAng, &p1x, &p1y); // intercept line shifted R toward the counter side + fwPerpOffset(px, py, arcRtmp, legRad, -(float)dirTmp, &b0x, &b0y); + // gamma >= 20 deg keeps the lines well separated + if (fwLineIntersect(p1x, p1y, d1x, d1y, b0x, b0y, ux, uy, 0.17f, &o2x, &o2y)) { const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; const float legLen = calc_length_pythagorean_2D(px - pos->x, py - pos->y); // roll-out tangency must lie ahead of us and leave straight leg to the WP if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * arcRtmp) { - intoEx = o2x - arcRtmp * cos_approx(nAng); // pickup = tangency on the intercept line - intoEy = o2y - arcRtmp * sin_approx(nAng); + fwPolarOffset(o2x, o2y, -arcRtmp, nAng, &intoEx, &intoEy); // pickup = tangency on the intercept line intoO2x = o2x; intoO2y = o2y; intoR = arcRtmp; intoBOut = legBearing; intoDir = -dirTmp; fwArcEngaged = true; @@ -821,7 +814,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcDir = dirTmp; arcCx = cx; arcCy = cy; - arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(icptRad)))); + arcOutBearing = fwRadToBearingCd(icptRad); phiNomCd = phiTmp; tEaseMs = tTmp; intoStage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over @@ -834,9 +827,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float phiT = acos_approx(constrainf(arcRtmp / dCP, 0.0f, 1.0f)); for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP const float th = alphaCP + (float)s * phiT; - const float tx = cx + arcRtmp * cos_approx(th); - const float ty = cy + arcRtmp * sin_approx(th); - if ((px - tx) * (-dirTmp * sin_approx(th)) + (py - ty) * (dirTmp * cos_approx(th)) > 0.0f) { + float tx, ty, tdx, tdy; + fwPolarOffset(cx, cy, arcRtmp, th, &tx, &ty); + fwTangentDir(th, dirTmp, &tdx, &tdy); + if ((px - tx) * tdx + (py - ty) * tdy > 0.0f) { fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; @@ -845,7 +839,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcDir = dirTmp; arcCx = cx; arcCy = cy; - arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - ty, px - tx))))); + arcOutBearing = fwRadToBearingCd(atan2_approx(py - ty, px - tx)); phiNomCd = phiTmp; tEaseMs = tTmp; } @@ -856,7 +850,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const int32_t hdgErr = wrap_18000(legBearing - cog); if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float phiTmp = fwBankForRadiusCd(v, arcRtmp); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); @@ -886,18 +880,14 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - const float d1x = cos_approx(cogRad), d1y = sin_approx(cogRad); - const float d2x = cos_approx(legRad), d2y = sin_approx(legRad); - const float cross = d1x * d2y - d1y * d2x; - const float p1x = pos->x + arcRtmp * cos_approx(cogRad + arcDir * (M_PIf * 0.5f)); - const float p1y = pos->y + arcRtmp * sin_approx(cogRad + arcDir * (M_PIf * 0.5f)); - if (fabsf(cross) > 0.087f) { // legs not near-parallel (30..160 deg turn) - const float p2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(legRad + arcDir * (M_PIf * 0.5f)); - const float p2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(legRad + arcDir * (M_PIf * 0.5f)); - const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross; - arcCx = p1x + tt * d1x; - arcCy = p1y + tt * d1y; - } else { // degenerate -> tangent at the entry point + float d1x, d1y, d2x, d2y, p1x, p1y, p2x, p2y; + fwBearingUnit(cog, &d1x, &d1y); + fwBearingUnit(legBearing, &d2x, &d2y); + fwPerpOffset(pos->x, pos->y, arcRtmp, cogRad, arcDir, &p1x, &p1y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + arcRtmp, legRad, arcDir, &p2x, &p2y); + // legs not near-parallel (30..160 deg turn); degenerate -> tangent at the entry point + if (!fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, 0.087f, &arcCx, &arcCy)) { arcCx = p1x; arcCy = p1y; } @@ -913,34 +903,35 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float phiTmp = fwBankForRadiusCd(v, arcRtmp); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const int8_t dirM = (nta > 0) ? 1 : -1; const int32_t bOut = wrap_36000(legBearing + nta); const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); const float bInRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - const float ux = cos_approx(bInRad), uy = sin_approx(bInRad); + float ux, uy; + fwBearingUnit(legBearing, &ux, &uy); // Main circle pinned at the WP, counter circle on the inbound leg; centers // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room const float Ls = 2.0f * v * (tTmp / 1000.0f); - const float o2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bOutRad + dirM * (M_PIf * 0.5f)); - const float o2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bOutRad + dirM * (M_PIf * 0.5f)); - const float ax = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bInRad - dirM * (M_PIf * 0.5f)); - const float ay = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bInRad - dirM * (M_PIf * 0.5f)); + float o2x, o2y, ax, ay; + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + arcRtmp, bOutRad, dirM, &o2x, &o2y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + arcRtmp, bInRad, -(float)dirM, &ax, &ay); const float wx = o2x - ax, wy = o2y - ay; const float wu = wx * ux + wy * uy; const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * arcRtmp * arcRtmp + Ls * Ls; if (disc > 0.0f) { const float s = wu - fast_fsqrtf(disc); // signed along-leg offset of the S start from the WP - const float triggerDist = -s + 1.5f * v * (tTmp / 1000.0f); + const float triggerDist = -s + fwRollInLeadCm(v, tTmp); if (s < 0.0f && posControl.wpDistance < triggerDist) { const float o1x = ax + s * ux; const float o1y = ay + s * uy; const float cAng = atan2_approx(o2y - o1y, o2x - o1x); const float beta = atan2_approx(Ls, 2.0f * arcRtmp); const float nAng = cAng + (float)dirM * beta; - intoEx = o2x - arcRtmp * cos_approx(nAng); // main-arc pickup = internal-tangent touch on the main circle - intoEy = o2y - arcRtmp * sin_approx(nAng); + fwPolarOffset(o2x, o2y, -arcRtmp, nAng, &intoEx, &intoEy); // main-arc pickup = internal-tangent touch on the main circle intoO2x = o2x; intoO2y = o2y; intoR = arcRtmp; intoBOut = bOut; intoDir = dirM; fwArcEngaged = true; @@ -951,8 +942,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcDir = -dirM; // counter-arc first arcCx = o1x; arcCy = o1y; - arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES( - cAng - (float)dirM * (M_PIf * 0.5f - beta))))); // internal-tangent course: the away arc rolls out onto it + // internal-tangent course: the away arc rolls out onto it + arcOutBearing = fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)); phiNomCd = phiTmp; tEaseMs = tTmp; intoStage = FW_INTO_AWAY; @@ -986,8 +977,9 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // Pick up the second arc at its tangency point. Along-track distance so a lateral residual // cannot miss it; the heading gate blocks the trigger early in the first arc, where the // pickup point still lies behind the exit course. - const float outRadE = CENTIDEGREES_TO_RADIANS((float)arcOutBearing); - fwArcPickupAlong = (intoEx - pos->x) * cos_approx(outRadE) + (intoEy - pos->y) * sin_approx(outRadE); + float outUx, outUy; + fwBearingUnit(arcOutBearing, &outUx, &outUy); + fwArcPickupAlong = (intoEx - pos->x) * outUx + (intoEy - pos->y) * outUy; // The away arc blocks the hand-back, so a pickup that never triggers strands the aircraft with // path tracking suppressed. Bound it by the time to fly half the away circle. @@ -1005,7 +997,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (intoStage == FW_INTO_AWAY && ABS(wrap_18000(arcOutBearing - cog)) < 4500 - && fwArcPickupAlong <= 1.5f * v * (tEaseMs / 1000.0f)) { + && fwArcPickupAlong <= fwRollInLeadCm(v, tEaseMs)) { phase = ARC_RAMP_IN; rampMs = 0.0f; rampStartCd = fwArcBankCmd; // blend from the current bank @@ -1015,9 +1007,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // an along-track shift instead of a parallel roll-out offset const float r2 = getFwCoordinatedTurnRadius(); const float legR2 = CENTIDEGREES_TO_RADIANS((float)intoBOut); - const float u2x = cos_approx(legR2), u2y = sin_approx(legR2); - const float b0x = posControl.activeWaypoint.pos.x + r2 * cos_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); - const float b0y = posControl.activeWaypoint.pos.y + r2 * sin_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); + float u2x, u2y, b0x, b0y; + fwBearingUnit(intoBOut, &u2x, &u2y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + r2, legR2, (float)intoDir, &b0x, &b0y); const float w2x = pos->x - b0x, w2y = pos->y - b0y; const float w2u = w2x * u2x + w2y * u2y; const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; @@ -1026,7 +1019,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcR = r2; arcCx = b0x + t2 * u2x; arcCy = b0y + t2 * u2y; - phiNomCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * r2))); + phiNomCd = fwBankForRadiusCd(v, r2); tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiNomCd)); } else { // drifted beyond the line: keep the planned circle arcR = intoR; @@ -1048,9 +1041,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with // the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. if (arcToLegLine && navConfig()->fw.wp_tracking_accuracy) { - const float legRadT = CENTIDEGREES_TO_RADIANS((float)legBearing); - const float offLeg = (pos->x - posControl.activeWaypoint.pos.x) * (-sin_approx(legRadT)) - + (pos->y - posControl.activeWaypoint.pos.y) * cos_approx(legRadT); + float legUx, legUy; + fwBearingUnit(legBearing, &legUx, &legUy); + const float offLeg = fwOffLegCm(pos->x, pos->y, + posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, legUx, legUy); const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); arcOutBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); } @@ -1072,8 +1066,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float rampSpanCd = fabsf((float)arcDir * phiNomCd - rampStartCd); const float rampDurMs = MAX(tEaseMs * rampSpanCd / MAX(phiNomCd, 1.0f), 0.5f * tEaseMs); const float p = (rampDurMs > 1.0f) ? constrainf(rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; - const float s = p * p * (3.0f - 2.0f * p); // smoothstep up - fwArcBankCmd = rampStartCd + ((float)arcDir * phiNomCd - rampStartCd) * s; + fwArcBankCmd = fwSmoothBlend(rampStartCd, (float)arcDir * phiNomCd, p); // smoothstep up if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle phase = ARC_STEADY; steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc @@ -1086,19 +1079,20 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float dy = pos->y - arcCy; const float eR = calc_length_pythagorean_2D(dx, dy) - arcR; // [cm], + = outside the arc const float alpha = atan2_approx(dy, dx); // azimuth on the arc - const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); + float tgx, tgy; + fwTangentDir(alpha, arcDir, &tgx, &tgy); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(tgy, tgx)))); const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent // FF from current groundspeed: wind changes v along the arc - const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcR))); + const float phiLiveCd = fwBankForRadiusCd(v, arcR); const float steadyCd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; steadyMs += US2S(deltaMicros) * 1000.0f; const float q = (tEaseMs > 1.0f) ? constrainf(steadyMs / tEaseMs, 0.0f, 1.0f) : 1.0f; - const float s = q * q * (3.0f - 2.0f * q); // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre // and eH can invert between two updates, which would otherwise go straight to the servos - const float blended = steadyStartCd + (steadyCd - steadyStartCd) * s; - const float maxStepCd = phiNomCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEaseMs, 1.0f); - fwArcBankCmd += constrainf(blended - fwArcBankCmd, -maxStepCd, maxStepCd); + const float blended = fwSmoothBlend(steadyStartCd, steadyCd, q); + const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, US2S(deltaMicros) * 1000.0f); + fwArcBankCmd = fwSlewToward(fwArcBankCmd, blended, maxStepCd); if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it phase = ARC_CAPTURE; @@ -1115,8 +1109,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); - const float maxStepCd = phiNomCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEaseMs, 1.0f); - fwArcBankCmd += constrainf(cmd - fwArcBankCmd, -maxStepCd, maxStepCd); + const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, US2S(deltaMicros) * 1000.0f); + fwArcBankCmd = fwSlewToward(fwArcBankCmd, cmd, maxStepCd); // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and // path tracking a moment of control while we sit a full turn diameter off the leg. if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= NAV_FW_ARC_EXIT_BANK_CD @@ -1165,7 +1159,9 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) const float arcRadius = MAX(fwActiveLoiterRadius, (float)NAV_FW_TURN_RADIUS_MIN); const int8_t dir = loiterDirection(); const float alpha = atan2_approx(dcy, dcx); - const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(dir * cos_approx(alpha), -dir * sin_approx(alpha))))); + float tgx, tgy; + fwTangentDir(alpha, dir, &tgx, &tgy); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(tgy, tgx)))); const int32_t eH = wrap_18000(tangentBearing - posControl.actualState.cog); const float eR = dist - arcRadius; @@ -1183,11 +1179,11 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) } const float v = posControl.actualState.velXY; - const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRadius))); + const float phiLiveCd = fwBankForRadiusCd(v, arcRadius); const float targetCd = dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); - const float maxStepCd = phiLiveCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEase, 1.0f); - fwArcBankCmd += constrainf(targetCd - fwArcBankCmd, -maxStepCd, maxStepCd); + const float maxStepCd = fwArcMaxStepCd(phiLiveCd, tEase, US2S(deltaMicros) * 1000.0f); + fwArcBankCmd = fwSlewToward(fwArcBankCmd, targetCd, maxStepCd); const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); @@ -1262,8 +1258,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t // We are closing in on a waypoint, calculate circular loiter if required if (needToCalculateCircularLoiter) { float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterTurnDirection * 45.0f); - float loiterTargetX = loiterCenterPos.x + navLoiterRadius * cos_approx(loiterAngle); - float loiterTargetY = loiterCenterPos.y + navLoiterRadius * sin_approx(loiterAngle); + float loiterTargetX, loiterTargetY; + fwPolarOffset(loiterCenterPos.x, loiterCenterPos.y, navLoiterRadius, loiterAngle, &loiterTargetX, &loiterTargetY); // We have temporary loiter target. Recalculate distance and position error posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; @@ -1354,11 +1350,11 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* Calculate cross track error */ posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + float wpUx, wpUy; + fwBearingUnit(posControl.activeWaypoint.bearing, &wpUx, &wpUy); 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)); + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - posControl.wpDistance * wpUx; + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - posControl.wpDistance * wpUy; navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); /* If waypoint tracking enabled force craft toward and closely track along waypoint course line. @@ -1399,9 +1395,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta previousCrossTrackError = navCrossTrackError; previousCrossTrackErrorUpdateTime = currentTimeUs; const fpVector3_t *trackPos = &navGetCurrentActualPositionAndVelocity()->pos; - const float legRad = CENTIDEGREES_TO_RADIANS((float)posControl.activeWaypoint.bearing); - const float offLeg = (trackPos->x - virtualCoursePoint.x) * (-sin_approx(legRad)) - + (trackPos->y - virtualCoursePoint.y) * cos_approx(legRad); + float legUx, legUy; + fwBearingUnit(posControl.activeWaypoint.bearing, &legUx, &legUy); + const float offLeg = fwOffLegCm(trackPos->x, trackPos->y, + virtualCoursePoint.x, virtualCoursePoint.y, legUx, legUy); const float cogOffRad = CENTIDEGREES_TO_RADIANS((float)wrap_18000(posControl.actualState.cog - posControl.activeWaypoint.bearing)); crossTrackErrorRate = -SIGN(offLeg) * posControl.actualState.velXY * sin_approx(cogOffRad); pt1FilterReset(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate); diff --git a/src/main/navigation/navigation_fixedwing_turn_math.c b/src/main/navigation/navigation_fixedwing_turn_math.c new file mode 100644 index 00000000000..77bb49663a2 --- /dev/null +++ b/src/main/navigation/navigation_fixedwing_turn_math.c @@ -0,0 +1,116 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/maths.h" + +#include "sensors/acceleration.h" + +#include "navigation/navigation_fixedwing_turn_math.h" + +// Coordinated bank [centideg] holding radius R at groundspeed v: phi = atan(v^2 / (g*R)) +float fwBankForRadiusCd(float v, float radiusCm) +{ + return DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * radiusCm))); +} + +// Course angle [rad] -> wrapped compass bearing [centideg] +int32_t fwRadToBearingCd(float rad) +{ + return wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(rad)))); +} + +// Unit tangent of a circle at azimuth alpha, in the direction of travel (dirF = +1 CW / -1 CCW) +void fwTangentDir(float alpha, float dirF, float *tx, float *ty) +{ + *tx = -dirF * sin_approx(alpha); + *ty = dirF * cos_approx(alpha); +} + +// Unit vector along a compass bearing [centideg]; the left normal is (-uy, ux) at the call site +void fwBearingUnit(int32_t bearingCd, float *ux, float *uy) +{ + const float rad = CENTIDEGREES_TO_RADIANS((float)bearingCd); + *ux = cos_approx(rad); + *uy = sin_approx(rad); +} + +// Point offset by distance d along angle angRad +void fwPolarOffset(float px, float py, float d, float angRad, float *ox, float *oy) +{ + *ox = px + d * cos_approx(angRad); + *oy = py + d * sin_approx(angRad); +} + +// Turn centre: offset r perpendicular to headingRad, toward the turn direction dirF +void fwPerpOffset(float px, float py, float r, float headingRad, float dirF, float *ox, float *oy) +{ + fwPolarOffset(px, py, r, headingRad + dirF * (M_PIf * 0.5f), ox, oy); +} + +// Intersection of the lines p1 + t*d1 and p2 + s*d2; false (outputs untouched) when near-parallel +bool fwLineIntersect(float p1x, float p1y, float d1x, float d1y, + float p2x, float p2y, float d2x, float d2y, + float minAbsCross, float *ox, float *oy) +{ + const float cross = d1x * d2y - d1y * d2x; + if (fabsf(cross) <= minAbsCross) { + return false; + } + + const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross; + *ox = p1x + tt * d1x; + *oy = p1y + tt * d1y; + return true; +} + +// Smoothstep interpolation from -> to over the normalised progress p. +// NOINLINE (F7/H7 only): measured smaller than letting LTO re-inline it at every call site +NOINLINE float fwSmoothBlend(float from, float to, float p) +{ + const float s = p * p * (3.0f - 2.0f * p); + return from + (to - from) * s; +} + +// Rate-limited approach of cur toward target (NOINLINE for the same reason as fwSmoothBlend) +NOINLINE float fwSlewToward(float cur, float target, float maxStep) +{ + return cur + constrainf(target - cur, -maxStep, maxStep); +} + +// Bank slew ceiling [centideg per tick]: the full nominal bank spread over one roll ease window +float fwArcMaxStepCd(float phiCd, float tEaseMs, float dtMs) +{ + return phiCd * dtMs / MAX(tEaseMs, 1.0f); +} + +// Signed cross-track offset [cm] of p from the line through q with unit direction (ux, uy) +float fwOffLegCm(float px, float py, float qx, float qy, float ux, float uy) +{ + return (px - qx) * (-uy) + (py - qy) * ux; +} + +// Ground distance covered while the roll-in ramp builds the bank (k calibrated in flight) +float fwRollInLeadCm(float v, float tMs) +{ + return 1.5f * v * (tMs / 1000.0f); +} diff --git a/src/main/navigation/navigation_fixedwing_turn_math.h b/src/main/navigation/navigation_fixedwing_turn_math.h new file mode 100644 index 00000000000..807624a90b6 --- /dev/null +++ b/src/main/navigation/navigation_fixedwing_turn_math.h @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include + +/* Shared turn-geometry primitives of the fixed-wing turn coordinator. + * + * The expressions below are frozen: navigation_fixedwing_turn_math_unittest.cc + * checks them bit-exactly against the inline code they replaced, so operand + * order, the (x / RAD) * 100 unit idiom and the lrintf placement are part of + * the contract, not style. Changing any of them changes flight behaviour. + */ + +float fwBankForRadiusCd(float v, float radiusCm); +int32_t fwRadToBearingCd(float rad); +void fwTangentDir(float alpha, float dirF, float *tx, float *ty); +void fwBearingUnit(int32_t bearingCd, float *ux, float *uy); +void fwPolarOffset(float px, float py, float d, float angRad, float *ox, float *oy); +void fwPerpOffset(float px, float py, float r, float headingRad, float dirF, float *ox, float *oy); +bool fwLineIntersect(float p1x, float p1y, float d1x, float d1y, + float p2x, float p2y, float d2x, float d2y, + float minAbsCross, float *ox, float *oy); +float fwSmoothBlend(float from, float to, float p); +float fwSlewToward(float cur, float target, float maxStep); +float fwArcMaxStepCd(float phiCd, float tEaseMs, float dtMs); +float fwOffLegCm(float px, float py, float qx, float qy, float ux, float uy); +float fwRollInLeadCm(float v, float tMs); diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 95d75fc335c..4c44a23dee6 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -28,6 +28,9 @@ set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") +set_property(SOURCE navigation_fixedwing_turn_math_unittest.cc PROPERTY depends + "navigation/navigation_fixedwing_turn_math.c" "common/maths.c") + set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c") set_property(SOURCE rcdevice_unittest.cc PROPERTY definitions USE_RCDEVICE) diff --git a/src/test/unit/navigation_fixedwing_turn_math_unittest.cc b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc new file mode 100644 index 00000000000..47d85b15a1b --- /dev/null +++ b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc @@ -0,0 +1,379 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +/* Equivalence tests for the turn-geometry helpers extracted from + * navigation_fixedwing.c. Every ref_* function below is a verbatim copy of the + * inline expression the helper replaced (the comment gives the call site at + * d1a87ed6c). The comparison is bit-exact, not approximate: the turn coordinator + * latches on centidegree thresholds, so a 1 ulp drift can change a phase + * transition. A failure here means the refactor changed flight behaviour. + */ + +#include +#include +#include + +extern "C" { +#include "platform.h" +#include "common/maths.h" +#include "common/time.h" +#include "sensors/acceleration.h" +#include "navigation/navigation_fixedwing_turn_math.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +// Bit-exact float comparison: NaN-safe and sensitive to the sign of zero +static bool bitsEqual(float a, float b) +{ + return memcmp(&a, &b, sizeof(float)) == 0; +} + +#define EXPECT_BITS_EQ(a, b) EXPECT_TRUE(bitsEqual((a), (b))) \ + << "got " << (a) << " want " << (b) + +// ---------------------------------------------------------------- references + +// P1 @610, 779, 859, 916, 1029, 1092, 1186 +static float ref_bankForRadiusCd(float v, float r) +{ + return DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * r))); +} + +// P2 @775, 824, 848, 954 +static int32_t ref_radToBearingCd(float rad) +{ + return wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(rad)))); +} + +// P2b @839 (dot-product form) and @1089/1168 (atan2 form) +static void ref_tangentDir(float th, int8_t dir, float *tx, float *ty) +{ + *tx = -dir * sin_approx(th); + *ty = dir * cos_approx(th); +} + +// P3 @791-792, 889-890, 921-922, 989-990, 1017-1018, 1051, 1402 +static void ref_bearingUnit(int32_t bearingCd, float *ux, float *uy) +{ + const float legRad = CENTIDEGREES_TO_RADIANS((float)bearingCd); + *ux = cos_approx(legRad); + *uy = sin_approx(legRad); +} + +// P3 @1359-1361: pre-existing site, no explicit (float) cast on the bearing +static void ref_bearingUnitUncast(int32_t bearingCd, float *ux, float *uy) +{ + *ux = cos_approx(CENTIDEGREES_TO_RADIANS(bearingCd)); + *uy = sin_approx(CENTIDEGREES_TO_RADIANS(bearingCd)); +} + +// P4b @798-799, 837-838, 1265-1266 +static void ref_polarOffset(float px, float py, float d, float a, float *ox, float *oy) +{ + *ox = px + d * cos_approx(a); + *oy = py + d * sin_approx(a); +} + +// P4b @812-813, 942-943: the subtracting form, replaced by passing -d +static void ref_polarOffsetMinus(float px, float py, float d, float a, float *ox, float *oy) +{ + *ox = px - d * cos_approx(a); + *oy = py - d * sin_approx(a); +} + +// P4 @785-786, 892-893, 895-896, 926-927, 1019-1020 +static void ref_perpOffsetPlus(float px, float py, float r, float h, int8_t dir, float *ox, float *oy) +{ + *ox = px + r * cos_approx(h + dir * (M_PIf * 0.5f)); + *oy = py + r * sin_approx(h + dir * (M_PIf * 0.5f)); +} + +// P4 @801-802, 928-929: the "-" sites, replaced by passing -dirF +static void ref_perpOffsetMinus(float px, float py, float r, float h, int8_t dir, float *ox, float *oy) +{ + *ox = px + r * cos_approx(h - (float)dir * (M_PIf * 0.5f)); + *oy = py + r * sin_approx(h - (float)dir * (M_PIf * 0.5f)); +} + +// P5 @803-807 / 890-903 +static bool ref_lineIntersect(float p1x, float p1y, float d1x, float d1y, + float p2x, float p2y, float d2x, float d2y, + float minAbsCross, float *ox, float *oy) +{ + const float cross = d1x * d2y - d1y * d2x; + if (fabsf(cross) > minAbsCross) { + const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross; + *ox = p1x + tt * d1x; + *oy = p1y + tt * d1y; + return true; + } + return false; +} + +// P7 @453-454: smoothstep with the multiply written the other way round +static float ref_smoothBlendSFirst(float from, float to, float p) +{ + const float s = p * p * (3.0f - 2.0f * p); + return from + s * (to - from); +} + +// P7 @702-703, 1075-1076, 1096+1099 +static float ref_smoothBlend(float from, float to, float p) +{ + const float s = p * p * (3.0f - 2.0f * p); + return from + (to - from) * s; +} + +// P8 @616-618, 1100-1101, 1118-1119, 1189-1190 +static float ref_slewToward(float cur, float target, float maxStep) +{ + float out = cur; + out += constrainf(target - out, -maxStep, maxStep); + return out; +} + +// P8 @1100, 1118, 1189 +static float ref_arcMaxStepCd(float phiCd, timeDelta_t deltaMicros, float tEaseMs) +{ + return phiCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEaseMs, 1.0f); +} + +// P13 @1051-1053, 1402-1404 +static float ref_offLegCm(float px, float py, float qx, float qy, float legRad) +{ + return (px - qx) * (-sin_approx(legRad)) + (py - qy) * cos_approx(legRad); +} + +// P17 @784, 935, 1008 +static float ref_rollInLeadCm(float v, float tMs) +{ + return 1.5f * v * (tMs / 1000.0f); +} + +// -------------------------------------------------------------------- sweeps + +// Groundspeed 5..60 m/s in cm/s, turn radius 10..300 m in cm +static const float SPEEDS_CMS[] = { 500.0f, 733.0f, 1000.0f, 1417.0f, 2000.0f, + 2750.0f, 3500.0f, 4333.0f, 5000.0f, 6000.0f }; +static const float RADII_CM[] = { 1000.0f, 1637.0f, 2500.0f, 4000.0f, 6500.0f, + 10000.0f, 15500.0f, 21000.0f, 27000.0f, 30000.0f }; + +TEST(NavFwTurnMathUnittest, BankForRadius) +{ + for (float v : SPEEDS_CMS) { + for (float r : RADII_CM) { + EXPECT_BITS_EQ(fwBankForRadiusCd(v, r), ref_bankForRadiusCd(v, r)); + } + } +} + +TEST(NavFwTurnMathUnittest, RadToBearing) +{ + for (int32_t cd = 0; cd < 36000; cd += 37) { + const float rad = CENTIDEGREES_TO_RADIANS((float)cd); + EXPECT_EQ(fwRadToBearingCd(rad), ref_radToBearingCd(rad)); + } + // outside one turn and negative, where wrap_36000 does the work + for (float rad = -12.0f; rad <= 12.0f; rad += 0.013f) { + EXPECT_EQ(fwRadToBearingCd(rad), ref_radToBearingCd(rad)); + } +} + +TEST(NavFwTurnMathUnittest, TangentDir) +{ + for (int32_t cd = 0; cd < 36000; cd += 37) { + const float alpha = CENTIDEGREES_TO_RADIANS((float)cd - 18000.0f); + for (int8_t dir = -1; dir <= 1; dir += 2) { + float tx, ty, rx, ry; + fwTangentDir(alpha, dir, &tx, &ty); + ref_tangentDir(alpha, dir, &rx, &ry); + EXPECT_BITS_EQ(tx, rx); + EXPECT_BITS_EQ(ty, ry); + // the @1089/1168 consumers feed the pair to atan2_approx(y, x) + EXPECT_BITS_EQ(atan2_approx(ty, tx), atan2_approx(ry, rx)); + } + } +} + +TEST(NavFwTurnMathUnittest, BearingUnit) +{ + for (int32_t cd = 0; cd < 36000; cd += 37) { + float ux, uy, rx, ry; + fwBearingUnit(cd, &ux, &uy); + ref_bearingUnit(cd, &rx, &ry); + EXPECT_BITS_EQ(ux, rx); + EXPECT_BITS_EQ(uy, ry); + ref_bearingUnitUncast(cd, &rx, &ry); + EXPECT_BITS_EQ(ux, rx); + EXPECT_BITS_EQ(uy, ry); + } +} + +TEST(NavFwTurnMathUnittest, PolarOffset) +{ + const float px = -13750.0f, py = 92100.0f; + for (int32_t cd = 0; cd < 36000; cd += 37) { + const float a = CENTIDEGREES_TO_RADIANS((float)cd - 18000.0f); + for (float d : RADII_CM) { + float ox, oy, rx, ry; + fwPolarOffset(px, py, d, a, &ox, &oy); + ref_polarOffset(px, py, d, a, &rx, &ry); + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + // the two "p - d * cos(a)" sites pass -d instead + fwPolarOffset(px, py, -d, a, &ox, &oy); + ref_polarOffsetMinus(px, py, d, a, &rx, &ry); + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + // 2R form used at @798-799 + fwPolarOffset(px, py, 2.0f * d, a, &ox, &oy); + ref_polarOffset(px, py, 2.0f * d, a, &rx, &ry); + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + } + } +} + +TEST(NavFwTurnMathUnittest, PerpOffset) +{ + const float px = 48320.0f, py = -7710.0f; + for (int32_t cd = 0; cd < 36000; cd += 37) { + const float h = CENTIDEGREES_TO_RADIANS((float)cd); + for (float r : RADII_CM) { + for (int8_t dir = -1; dir <= 1; dir += 2) { + float ox, oy, rx, ry; + fwPerpOffset(px, py, r, h, dir, &ox, &oy); + ref_perpOffsetPlus(px, py, r, h, dir, &rx, &ry); + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + fwPerpOffset(px, py, r, h, -(float)dir, &ox, &oy); + ref_perpOffsetMinus(px, py, r, h, dir, &rx, &ry); + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + } + } + } +} + +TEST(NavFwTurnMathUnittest, LineIntersect) +{ + const float p1x = 1200.0f, p1y = -3400.0f; + const float p2x = -9100.0f, p2y = 26500.0f; + for (int32_t a = 0; a < 36000; a += 337) { + for (int32_t b = 0; b < 36000; b += 331) { + float d1x, d1y, d2x, d2y; + fwBearingUnit(a, &d1x, &d1y); + fwBearingUnit(b, &d2x, &d2y); + for (float minCross : { 0.087f, 0.17f }) { + float ox = 0.0f, oy = 0.0f, rx = 0.0f, ry = 0.0f; + const bool got = fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, minCross, &ox, &oy); + const bool want = ref_lineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, minCross, &rx, &ry); + EXPECT_EQ(got, want); + if (want) { + EXPECT_BITS_EQ(ox, rx); + EXPECT_BITS_EQ(oy, ry); + } + } + } + } +} + +TEST(NavFwTurnMathUnittest, LineIntersectDegenerate) +{ + const float p1x = 0.0f, p1y = 0.0f, p2x = 500.0f, p2y = 500.0f; + float ox = 1.0f, oy = 2.0f; + + // exactly parallel and exactly anti-parallel + EXPECT_FALSE(fwLineIntersect(p1x, p1y, 1.0f, 0.0f, p2x, p2y, 1.0f, 0.0f, 0.087f, &ox, &oy)); + EXPECT_FALSE(fwLineIntersect(p1x, p1y, 1.0f, 0.0f, p2x, p2y, -1.0f, 0.0f, 0.087f, &ox, &oy)); + EXPECT_BITS_EQ(ox, 1.0f); // outputs untouched on the degenerate path + EXPECT_BITS_EQ(oy, 2.0f); + + // cross exactly on / just below / just above each threshold + for (float thr : { 0.087f, 0.17f }) { + for (float cross : { thr, nextafterf(thr, 0.0f), nextafterf(thr, 1.0f), -thr, + nextafterf(-thr, 0.0f), nextafterf(-thr, -1.0f) }) { + float gx = 0.0f, gy = 0.0f, wx = 0.0f, wy = 0.0f; + // d1 = (1,0), d2 = (0,cross) -> d1x*d2y - d1y*d2x == cross + const bool got = fwLineIntersect(p1x, p1y, 1.0f, 0.0f, p2x, p2y, 0.0f, cross, thr, &gx, &gy); + const bool want = ref_lineIntersect(p1x, p1y, 1.0f, 0.0f, p2x, p2y, 0.0f, cross, thr, &wx, &wy); + EXPECT_EQ(got, want); + if (want) { + EXPECT_BITS_EQ(gx, wx); + EXPECT_BITS_EQ(gy, wy); + } + } + } +} + +TEST(NavFwTurnMathUnittest, SmoothBlend) +{ + for (float from : { -4500.0f, -1234.5f, 0.0f, 987.25f, 4500.0f }) { + for (float to : { -4500.0f, -321.75f, 0.0f, 2200.0f, 4500.0f }) { + for (float p = 0.0f; p <= 1.0f; p += 1.0f / 512.0f) { + EXPECT_BITS_EQ(fwSmoothBlend(from, to, p), ref_smoothBlend(from, to, p)); + EXPECT_BITS_EQ(fwSmoothBlend(from, to, p), ref_smoothBlendSFirst(from, to, p)); + } + } + } +} + +TEST(NavFwTurnMathUnittest, SlewToward) +{ + for (float cur : { -4500.0f, -700.0f, 0.0f, 55.5f, 4500.0f }) { + for (float target : { -4500.0f, -12.5f, 0.0f, 1800.0f, 4500.0f }) { + for (float step : { 0.0f, 0.5f, 17.25f, 300.0f, 99999.0f }) { + EXPECT_BITS_EQ(fwSlewToward(cur, target, step), ref_slewToward(cur, target, step)); + } + } + } +} + +TEST(NavFwTurnMathUnittest, ArcMaxStep) +{ + for (float phi : { 100.0f, 1500.0f, 3000.0f, 4500.0f }) { + for (float tEase : { 0.0f, 0.5f, 1.0f, 250.0f, 1400.0f }) { + for (timeDelta_t dt : { 1, 1000, 20000, 50000, 200000 }) { + EXPECT_BITS_EQ(fwArcMaxStepCd(phi, tEase, US2S(dt) * 1000.0f), + ref_arcMaxStepCd(phi, dt, tEase)); + } + } + } +} + +TEST(NavFwTurnMathUnittest, OffLeg) +{ + const float px = 12345.0f, py = -6789.0f; + const float qx = -2200.0f, qy = 33300.0f; + for (int32_t cd = 0; cd < 36000; cd += 37) { + float ux, uy; + fwBearingUnit(cd, &ux, &uy); + const float legRad = CENTIDEGREES_TO_RADIANS((float)cd); + EXPECT_BITS_EQ(fwOffLegCm(px, py, qx, qy, ux, uy), ref_offLegCm(px, py, qx, qy, legRad)); + } +} + +TEST(NavFwTurnMathUnittest, RollInLead) +{ + for (float v : SPEEDS_CMS) { + for (float t = 0.0f; t <= 2500.0f; t += 7.0f) { + EXPECT_BITS_EQ(fwRollInLeadCm(v, t), ref_rollInLeadCm(v, t)); + } + } +} From 9372af57e31798236742e7b097067aaaabe994b2 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 17:48:06 +0200 Subject: [PATCH 2/8] FW nav: hoist per-tick arc values, single-source the debug and clamp blocks dtMs, the heading error to the leg and the bank slew step were computed up to three times per tick; the landing-approach turn-mode rule, the bank clamp and the debug release were spelled out at two or three places each. The arc's debug channels are written under one mode test. Values and write order are unchanged. MATEKF722 -192 B flash. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_fixedwing.c | 92 +++++++++++++--------- 1 file changed, 53 insertions(+), 39 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 96a3710af20..256e54e5bbf 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -564,6 +564,13 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] } +// Landing approach always flies coordinated FLY_BY turns, whatever mode is configured +static navFwWpTurnMode_e fwEffectiveTurnMode(void) +{ + return (posControl.navState == NAV_STATE_FW_LANDING_APPROACH) ? NAV_FW_WP_TURN_COORD_FLY_BY + : navConfig()->fw.wp_turn_mode; +} + // Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. static float getFwCoordinatedTurnRadius(void) { @@ -578,8 +585,8 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr { const uint8_t ffGain = navConfig()->fw.turn_ff_gain; if (ffGain == 0 || posControl.actualState.velXY <= NAV_FW_TURN_MIN_SPEED - || (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT && posControl.navState != NAV_STATE_FW_LANDING_APPROACH)) { - return 0.0f; // DIRECT = pure legacy PID (landing approach forces FLY_BY) + || fwEffectiveTurnMode() == NAV_FW_WP_TURN_DIRECT) { + return 0.0f; // DIRECT = pure legacy PID } // Turn-context gate: arm on a leg change, disarm once aligned. A later heading error on the same @@ -700,6 +707,21 @@ static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) return fwSmoothBlend(fwArcHandbackCmdCd, rollTargetCd, p); } +// Clamp to the flyable ceiling: rate limits, handoff checks and the smoother seed must not +// run on a command the airframe cannot reach (wind can drive the radial error arbitrarily large) +static void fwArcClampBankCmd(void) +{ + const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); + fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); +} + +// Clear on release, else the log keeps showing the last arc phase for minutes +static void fwArcDebugRelease(void) +{ + DEBUG_SET(DEBUG_FW_TURN, 1, 0); + DEBUG_SET(DEBUG_FW_TURN, 3, 0); +} + // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive // capture roll-out. Sets fwArcActive (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) @@ -730,26 +752,23 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcActive = false; - // Landing approach always flies coordinated FLY_BY turns, whatever mode is configured - navFwWpTurnMode_e turnMode = navConfig()->fw.wp_turn_mode; - if (posControl.navState == NAV_STATE_FW_LANDING_APPROACH) { - turnMode = NAV_FW_WP_TURN_COORD_FLY_BY; - } + const navFwWpTurnMode_e turnMode = fwEffectiveTurnMode(); const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; if (turnMode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { fwArcEngaged = false; fwArcPrevLegBearing = -1; intoStage = FW_INTO_IDLE; - DEBUG_SET(DEBUG_FW_TURN, 1, 0); // clear, else the log keeps showing the last arc phase for minutes - DEBUG_SET(DEBUG_FW_TURN, 3, 0); + fwArcDebugRelease(); return; } const int32_t legBearing = posControl.activeWaypoint.bearing; const int32_t cog = posControl.actualState.cog; + const int32_t hdgErrToLeg = wrap_18000(legBearing - cog); const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float v = posControl.actualState.velXY; + const float dtMs = US2S(deltaMicros) * 1000.0f; if (!fwArcEngaged) { arcToLegLine = false; @@ -758,7 +777,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) * the whole turn unshaped. Unseeded + grossly off the leg course = a pending turn. */ const bool legChanged = (fwArcPrevLegBearing >= 0) ? (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) - : (ABS(wrap_18000(legBearing - cog)) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); + : (ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); fwArcPrevLegBearing = legBearing; if (legChanged) { intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry @@ -789,8 +808,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); float ux, uy; fwBearingUnit(legBearing, &ux, &uy); - const int32_t turnCd = wrap_18000(legBearing - cog); - const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(turnCd), 2000.0f, 4500.0f)); + const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(hdgErrToLeg), 2000.0f, 4500.0f)); const float icptRad = legRad + (float)dirTmp * gammaRad; const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); @@ -847,14 +865,13 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - const int32_t hdgErr = wrap_18000(legBearing - cog); - if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); const float phiTmp = fwBankForRadiusCd(v, arcRtmp); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); - if (capped || ABS(hdgErr) > NAV_FW_ARC_SHARP_TURN_CD) { + if (capped || ABS(hdgErrToLeg) > NAV_FW_ARC_SHARP_TURN_CD) { // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly fwArcEngaged = true; phase = ARC_CAPTURE; @@ -862,18 +879,18 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) rampMs = 0.0f; rampStartCd = 0.0f; arcR = arcRtmp; // capture ignores it, but the away timeout is sized from it - arcDir = (hdgErr > 0) ? 1 : -1; + arcDir = (hdgErrToLeg > 0) ? 1 : -1; arcOutBearing = legBearing; arcToLegLine = true; phiNomCd = phiTmp; tEaseMs = tTmp; - } else if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps + } else if (2.0f * psiTmp < (float)ABS(hdgErrToLeg)) { // enough turn left for a steady arc between the ease ramps fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; rampStartCd = fwLastNavRollCmdCd; arcR = arcRtmp; - arcDir = (hdgErr > 0) ? 1 : -1; + arcDir = (hdgErrToLeg > 0) ? 1 : -1; arcOutBearing = legBearing; phiNomCd = phiTmp; tEaseMs = tTmp; @@ -984,7 +1001,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // The away arc blocks the hand-back, so a pickup that never triggers strands the aircraft with // path tracking suppressed. Bound it by the time to fly half the away circle. if (intoStage == FW_INTO_AWAY) { - intoAwayMs += US2S(deltaMicros) * 1000.0f; + intoAwayMs += dtMs; if (intoAwayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * arcR / MAX(v, NAV_FW_TURN_MIN_SPEED)) { intoStage = FW_INTO_DONE; // release the block; the capture finishes on the leg arcOutBearing = legBearing; @@ -1049,7 +1066,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcOutBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); } - rampMs += US2S(deltaMicros) * 1000.0f; + rampMs += dtMs; fwArcEaseMs = tEaseMs; // published for the handback fade const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); @@ -1059,6 +1076,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (tEaseMs / 1000.0f); + const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, dtMs); switch (phase) { case ARC_RAMP_IN: { @@ -1086,12 +1104,11 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // FF from current groundspeed: wind changes v along the arc const float phiLiveCd = fwBankForRadiusCd(v, arcR); const float steadyCd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; - steadyMs += US2S(deltaMicros) * 1000.0f; + steadyMs += dtMs; const float q = (tEaseMs > 1.0f) ? constrainf(steadyMs / tEaseMs, 0.0f, 1.0f) : 1.0f; // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre // and eH can invert between two updates, which would otherwise go straight to the servos const float blended = fwSmoothBlend(steadyStartCd, steadyCd, q); - const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, US2S(deltaMicros) * 1000.0f); fwArcBankCmd = fwSlewToward(fwArcBankCmd, blended, maxStepCd); if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it @@ -1109,34 +1126,31 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); - const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, US2S(deltaMicros) * 1000.0f); fwArcBankCmd = fwSlewToward(fwArcBankCmd, cmd, maxStepCd); // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and // path tracking a moment of control while we sit a full turn diameter off the leg. if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= NAV_FW_ARC_EXIT_BANK_CD && intoStage != FW_INTO_AWAY) { // aligned and nearly level -> hand back fwArcEngaged = false; - DEBUG_SET(DEBUG_FW_TURN, 1, 0); // released: don't leave stale values in the log - DEBUG_SET(DEBUG_FW_TURN, 3, 0); + fwArcDebugRelease(); return; } break; } } - // Clamp to the flyable ceiling: rate limits, handoff checks and the smoother seed must not - // run on a command the airframe cannot reach (wind can drive eR arbitrarily large) - const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); - fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + fwArcClampBankCmd(); - DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(arcR)); // active arc radius [cm] - DEBUG_SET(DEBUG_FW_TURN, 1, (phase + 1) * 10 + intoStage); // coordinator state: (arc phase + 1)*10 + S stage - DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course [centideg] - DEBUG_SET(DEBUG_FW_TURN, 3, hdgErrOut); // remaining heading to the exit course [centideg] - DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // arc bank command [centideg] - DEBUG_SET(DEBUG_FW_TURN, 7, lrintf(tEaseMs)); // roll ease time [ms] -> sizes the turn leads - if (intoStage == FW_INTO_AWAY) { - DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwArcPickupAlong)); // away arc: along-track distance to the pickup [cm] + if (debugMode == DEBUG_FW_TURN) { + debug[0] = lrintf(arcR); // active arc radius [cm] + debug[1] = (phase + 1) * 10 + intoStage; // coordinator state: (arc phase + 1)*10 + S stage + debug[2] = arcOutBearing; // exit course [centideg] + debug[3] = hdgErrOut; // remaining heading to the exit course [centideg] + debug[4] = lrintf(fwArcBankCmd); // arc bank command [centideg] + debug[7] = lrintf(tEaseMs); // roll ease time [ms] -> sizes the turn leads + if (intoStage == FW_INTO_AWAY) { + debug[6] = lrintf(fwArcPickupAlong); // away arc: along-track distance to the pickup [cm] + } } fwArcActive = true; } @@ -1184,8 +1198,7 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); const float maxStepCd = fwArcMaxStepCd(phiLiveCd, tEase, US2S(deltaMicros) * 1000.0f); fwArcBankCmd = fwSlewToward(fwArcBankCmd, targetCd, maxStepCd); - const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); - fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + fwArcClampBankCmd(); DEBUG_SET(DEBUG_FW_TURN, 1, 40); // loiter circle controller engaged DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); @@ -1236,6 +1249,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; + // not fwEffectiveTurnMode(): the call costs 448 B of flash here at -O2 (F405), none at -Os const bool flyByLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_BY || posControl.navState == NAV_STATE_FW_LANDING_APPROACH; if (flyByLeg && waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { From 26c6470fba7dc8f7d4a456ceeaafbfc175c07800 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 18:04:15 +0200 Subject: [PATCH 3/8] FW nav: share the arc coordinator's transition and steady-law code The five ramp-in engages, three capture fallbacks and two S stagings each spelled out the same state assignments, the turn plan triple (radius, bank, ease time) was computed at three places, and the steady arc law existed once for the WP arc and once for the loiter circle. Each is now one function; the arc statics move to file scope so the helpers can write them. Values, evaluation order and read points are unchanged. MATEKF722 -256 B flash. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_fixedwing.c | 311 +++++++++++---------- 1 file changed, 159 insertions(+), 152 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 256e54e5bbf..b0cbf1878df 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -722,34 +722,102 @@ static void fwArcDebugRelease(void) DEBUG_SET(DEBUG_FW_TURN, 3, 0); } +// Arc coordinator state. At file scope only so the shared transition helpers below can write it; +// nothing outside updateFwTurnArc and those helpers touches any of it. +enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; +static uint8_t phase; +static float arcCx, arcCy, arcR; +static int8_t arcDir; +static int32_t arcOutBearing; +static bool arcToLegLine; // fallback capture: converge onto the leg line itself, not just its course +static float phiNomCd; // coordinated nominal bank for this turn [centideg] +static float tEaseMs; // roll-in ease time +static float rampMs; // elapsed time in the ramp-in phase +static float rampStartCd; // bank the ramp blends from (the live nav command on entry - a banked + // loiter exit must not level off first; the pickup blends mid-arc) +static float steadyMs; // elapsed time in the steady phase +static float steadyStartCd; // bank the steady law blends from (the ramp's final command) + +// S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, roll-reversal gap, second arc +enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; +static uint8_t intoStage; +static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) +static float intoO2x, intoO2y; // second-arc centre +static float intoR; +static int32_t intoBOut; +static int8_t intoDir; +static float intoAwayMs; // time spent in the away arc, to bound a pickup that never triggers + +// Radius, coordinated bank and roll ease time of a planned turn - never meaningful apart +typedef struct { + float r; + float phiNomCd; + float tEaseMs; +} fwTurnPlan_t; + +static void fwPlanTurn(float v, fwTurnPlan_t *plan) +{ + plan->r = getFwCoordinatedTurnRadius(); + plan->phiNomCd = fwBankForRadiusCd(v, plan->r); + plan->tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(plan->phiNomCd)); +} + +// Engage or re-anchor the arc on a circle and roll in from the bank currently being flown +static void fwArcEngageRampIn(const fwTurnPlan_t *plan, int8_t dir, float cx, float cy, + int32_t outBearing, float blendFromCd) +{ + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = blendFromCd; + arcR = plan->r; + arcDir = dir; + arcCx = cx; + arcCy = cy; + arcOutBearing = outBearing; + phiNomCd = plan->phiNomCd; + tEaseMs = plan->tEaseMs; +} + +// No usable circle geometry left: capture the leg line itself with the bounded roll-out +static void fwArcCaptureToLeg(int32_t legBearing) +{ + arcOutBearing = legBearing; + arcToLegLine = true; + phase = ARC_CAPTURE; +} + +// Stage the second arc of an S so the shared pickup logic can re-anchor and fly it +static void fwArcStageSecondArc(float o2x, float o2y, float nAng, float r, int32_t bOut, int8_t dir) +{ + fwPolarOffset(o2x, o2y, -r, nAng, &intoEx, &intoEy); // pickup = internal-tangent touch on the circle + intoO2x = o2x; intoO2y = o2y; + intoR = r; intoBOut = bOut; intoDir = dir; +} + +// Steady arc law: coordinated bank for the live speed (wind changes v along the arc) plus radial +// and tangent-heading feedback. Shared by the WP turn arc and the loiter circle. +static float fwArcLawCd(float px, float py, float cx, float cy, float r, int8_t dir, + int32_t cog, float v, float *eROut, int32_t *eHOut) +{ + const float dx = px - cx; + const float dy = py - cy; + const float eR = calc_length_pythagorean_2D(dx, dy) - r; // [cm], + = outside the arc + const float alpha = atan2_approx(dy, dx); // azimuth on the arc + float tgx, tgy; + fwTangentDir(alpha, dir, &tgx, &tgy); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(tgy, tgx)))); + const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent + const float phiLiveCd = fwBankForRadiusCd(v, r); + *eROut = eR; + *eHOut = eH; + return dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; +} + // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive // capture roll-out. Sets fwArcActive (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) { - enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; - static uint8_t phase; - static float arcCx, arcCy, arcR; - static int8_t arcDir; - static int32_t arcOutBearing; - static bool arcToLegLine; // fallback capture: converge onto the leg line itself, not just its course - static float phiNomCd; // coordinated nominal bank for this turn [centideg] - static float tEaseMs; // roll-in ease time - static float rampMs; // elapsed time in the ramp-in phase - static float rampStartCd; // bank the ramp blends from (the live nav command on entry - a banked - // loiter exit must not level off first; the pickup blends mid-arc) - static float steadyMs; // elapsed time in the steady phase - static float steadyStartCd; // bank the steady law blends from (the ramp's final command) - - // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, roll-reversal gap, second arc - enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; - static uint8_t intoStage; - static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) - static float intoO2x, intoO2y; // second-arc centre - static float intoR; - static int32_t intoBOut; - static int8_t intoDir; - static float intoAwayMs; // time spent in the away arc, to bound a pickup that never triggers - fwArcActive = false; const navFwWpTurnMode_e turnMode = fwEffectiveTurnMode(); @@ -791,16 +859,15 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const int32_t brgToWp = fwRadToBearingCd(atan2_approx(py - pos->y, px - pos->x)); const int32_t toWpErr = wrap_18000(brgToWp - cog); if (ABS(toWpErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = fwBankForRadiusCd(v, arcRtmp); - const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + fwTurnPlan_t plan; + fwPlanTurn(v, &plan); const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); // Pin ahead by the roll-in drift so the ramp ends ON the circle - const float leadDist = fwRollInLeadCm(v, tTmp); + const float leadDist = fwRollInLeadCm(v, plan.tEaseMs); float leadX, leadY, cx, cy; fwPolarOffset(pos->x, pos->y, leadDist, cogRad, &leadX, &leadY); - fwPerpOffset(leadX, leadY, arcRtmp, cogRad, dirTmp, &cx, &cy); + fwPerpOffset(leadX, leadY, plan.r, cogRad, dirTmp, &cx, &cy); if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { // Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the // leg, gamma = half the turn for shallow corners), short straight for the reverse @@ -813,101 +880,70 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); float p1x, p1y, b0x, b0y, o2x, o2y; - fwPolarOffset(cx, cy, 2.0f * arcRtmp, nAng, &p1x, &p1y); // intercept line shifted R toward the counter side - fwPerpOffset(px, py, arcRtmp, legRad, -(float)dirTmp, &b0x, &b0y); + fwPolarOffset(cx, cy, 2.0f * plan.r, nAng, &p1x, &p1y); // intercept line shifted R toward the counter side + fwPerpOffset(px, py, plan.r, legRad, -(float)dirTmp, &b0x, &b0y); // gamma >= 20 deg keeps the lines well separated if (fwLineIntersect(p1x, p1y, d1x, d1y, b0x, b0y, ux, uy, 0.17f, &o2x, &o2y)) { const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; const float legLen = calc_length_pythagorean_2D(px - pos->x, py - pos->y); // roll-out tangency must lie ahead of us and leave straight leg to the WP - if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * arcRtmp) { - fwPolarOffset(o2x, o2y, -arcRtmp, nAng, &intoEx, &intoEy); // pickup = tangency on the intercept line - intoO2x = o2x; intoO2y = o2y; - intoR = arcRtmp; intoBOut = legBearing; intoDir = -dirTmp; - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = fwLastNavRollCmdCd; - arcR = arcRtmp; - arcDir = dirTmp; - arcCx = cx; - arcCy = cy; - arcOutBearing = fwRadToBearingCd(icptRad); - phiNomCd = phiTmp; - tEaseMs = tTmp; + if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * plan.r) { + fwArcStageSecondArc(o2x, o2y, nAng, plan.r, legBearing, -dirTmp); + fwArcEngageRampIn(&plan, dirTmp, cx, cy, fwRadToBearingCd(icptRad), fwLastNavRollCmdCd); intoStage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over } } } const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); - if (!fwArcEngaged && dCP > 1.05f * arcRtmp) { // next WP outside the circle: a tangent exists + if (!fwArcEngaged && dCP > 1.05f * plan.r) { // next WP outside the circle: a tangent exists const float alphaCP = atan2_approx(py - cy, px - cx); - const float phiT = acos_approx(constrainf(arcRtmp / dCP, 0.0f, 1.0f)); + const float phiT = acos_approx(constrainf(plan.r / dCP, 0.0f, 1.0f)); for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP const float th = alphaCP + (float)s * phiT; float tx, ty, tdx, tdy; - fwPolarOffset(cx, cy, arcRtmp, th, &tx, &ty); + fwPolarOffset(cx, cy, plan.r, th, &tx, &ty); fwTangentDir(th, dirTmp, &tdx, &tdy); if ((px - tx) * tdx + (py - ty) * tdy > 0.0f) { - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = fwLastNavRollCmdCd; - arcR = arcRtmp; - arcDir = dirTmp; - arcCx = cx; - arcCy = cy; - arcOutBearing = fwRadToBearingCd(atan2_approx(py - ty, px - tx)); - phiNomCd = phiTmp; - tEaseMs = tTmp; + fwArcEngageRampIn(&plan, dirTmp, cx, cy, + fwRadToBearingCd(atan2_approx(py - ty, px - tx)), fwLastNavRollCmdCd); } } } } } if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = fwBankForRadiusCd(v, arcRtmp); - const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); - const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v - const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); + fwTurnPlan_t plan; + fwPlanTurn(v, &plan); + const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / plan.r)); // v/R == g*tan(phi)/v + const float psiTmp = 0.5f * omegaNomCds * (plan.tEaseMs / 1000.0f); if (capped || ABS(hdgErrToLeg) > NAV_FW_ARC_SHARP_TURN_CD) { // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly fwArcEngaged = true; - phase = ARC_CAPTURE; fwArcBankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step rampMs = 0.0f; rampStartCd = 0.0f; - arcR = arcRtmp; // capture ignores it, but the away timeout is sized from it + arcR = plan.r; // capture ignores it, but the away timeout is sized from it arcDir = (hdgErrToLeg > 0) ? 1 : -1; - arcOutBearing = legBearing; - arcToLegLine = true; - phiNomCd = phiTmp; - tEaseMs = tTmp; + phiNomCd = plan.phiNomCd; + tEaseMs = plan.tEaseMs; + fwArcCaptureToLeg(legBearing); } else if (2.0f * psiTmp < (float)ABS(hdgErrToLeg)) { // enough turn left for a steady arc between the ease ramps - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = fwLastNavRollCmdCd; - arcR = arcRtmp; - arcDir = (hdgErrToLeg > 0) ? 1 : -1; - arcOutBearing = legBearing; - phiNomCd = phiTmp; - tEaseMs = tTmp; + const int8_t dirTmp = (hdgErrToLeg > 0) ? 1 : -1; // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - float d1x, d1y, d2x, d2y, p1x, p1y, p2x, p2y; + float d1x, d1y, d2x, d2y, p1x, p1y, p2x, p2y, cx, cy; fwBearingUnit(cog, &d1x, &d1y); fwBearingUnit(legBearing, &d2x, &d2y); - fwPerpOffset(pos->x, pos->y, arcRtmp, cogRad, arcDir, &p1x, &p1y); + fwPerpOffset(pos->x, pos->y, plan.r, cogRad, dirTmp, &p1x, &p1y); fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - arcRtmp, legRad, arcDir, &p2x, &p2y); + plan.r, legRad, dirTmp, &p2x, &p2y); // legs not near-parallel (30..160 deg turn); degenerate -> tangent at the entry point - if (!fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, 0.087f, &arcCx, &arcCy)) { - arcCx = p1x; - arcCy = p1y; + if (!fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, 0.087f, &cx, &cy)) { + cx = p1x; + cy = p1y; } + fwArcEngageRampIn(&plan, dirTmp, cx, cy, legBearing, fwLastNavRollCmdCd); } } } @@ -919,9 +955,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (intoStage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - const float arcRtmp = getFwCoordinatedTurnRadius(); - const float phiTmp = fwBankForRadiusCd(v, arcRtmp); - const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + fwTurnPlan_t plan; + fwPlanTurn(v, &plan); const int8_t dirM = (nta > 0) ? 1 : -1; const int32_t bOut = wrap_36000(legBearing + nta); const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); @@ -930,39 +965,28 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwBearingUnit(legBearing, &ux, &uy); // Main circle pinned at the WP, counter circle on the inbound leg; centers // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room - const float Ls = 2.0f * v * (tTmp / 1000.0f); + const float Ls = 2.0f * v * (plan.tEaseMs / 1000.0f); float o2x, o2y, ax, ay; fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - arcRtmp, bOutRad, dirM, &o2x, &o2y); + plan.r, bOutRad, dirM, &o2x, &o2y); fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - arcRtmp, bInRad, -(float)dirM, &ax, &ay); + plan.r, bInRad, -(float)dirM, &ax, &ay); const float wx = o2x - ax, wy = o2y - ay; const float wu = wx * ux + wy * uy; - const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * arcRtmp * arcRtmp + Ls * Ls; + const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * plan.r * plan.r + Ls * Ls; if (disc > 0.0f) { const float s = wu - fast_fsqrtf(disc); // signed along-leg offset of the S start from the WP - const float triggerDist = -s + fwRollInLeadCm(v, tTmp); + const float triggerDist = -s + fwRollInLeadCm(v, plan.tEaseMs); if (s < 0.0f && posControl.wpDistance < triggerDist) { const float o1x = ax + s * ux; const float o1y = ay + s * uy; const float cAng = atan2_approx(o2y - o1y, o2x - o1x); - const float beta = atan2_approx(Ls, 2.0f * arcRtmp); + const float beta = atan2_approx(Ls, 2.0f * plan.r); const float nAng = cAng + (float)dirM * beta; - fwPolarOffset(o2x, o2y, -arcRtmp, nAng, &intoEx, &intoEy); // main-arc pickup = internal-tangent touch on the main circle - intoO2x = o2x; intoO2y = o2y; - intoR = arcRtmp; intoBOut = bOut; intoDir = dirM; - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = fwLastNavRollCmdCd; - arcR = arcRtmp; - arcDir = -dirM; // counter-arc first - arcCx = o1x; - arcCy = o1y; - // internal-tangent course: the away arc rolls out onto it - arcOutBearing = fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)); - phiNomCd = phiTmp; - tEaseMs = tTmp; + fwArcStageSecondArc(o2x, o2y, nAng, plan.r, bOut, dirM); + // counter-arc first, rolling out onto the internal-tangent course + fwArcEngageRampIn(&plan, -dirM, o1x, o1y, + fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)), fwLastNavRollCmdCd); intoStage = FW_INTO_AWAY; } } @@ -982,9 +1006,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500 && ABS(wrap_18000(legBearing - arcOutBearing)) > 500) { fwFlyByCappedLatch = false; - arcOutBearing = legBearing; - arcToLegLine = true; - phase = ARC_CAPTURE; + fwArcCaptureToLeg(legBearing); if (intoStage == FW_INTO_AWAY) { intoStage = FW_INTO_DONE; // staged S is stale: release the hand-back block } @@ -1004,9 +1026,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) intoAwayMs += dtMs; if (intoAwayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * arcR / MAX(v, NAV_FW_TURN_MIN_SPEED)) { intoStage = FW_INTO_DONE; // release the block; the capture finishes on the leg - arcOutBearing = legBearing; - arcToLegLine = true; - phase = ARC_CAPTURE; + fwArcCaptureToLeg(legBearing); } } else { intoAwayMs = 0.0f; @@ -1015,10 +1035,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (intoStage == FW_INTO_AWAY && ABS(wrap_18000(arcOutBearing - cog)) < 4500 && fwArcPickupAlong <= fwRollInLeadCm(v, tEaseMs)) { - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = fwArcBankCmd; // blend from the current bank - arcDir = intoDir; + const float blendFromCd = fwArcBankCmd; // blend from the bank flown right now // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the // circle re-anchored along the leg line through the actual position: wind drift becomes // an along-track shift instead of a parallel roll-out offset @@ -1031,19 +1048,23 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float w2x = pos->x - b0x, w2y = pos->y - b0y; const float w2u = w2x * u2x + w2y * u2y; const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; + fwTurnPlan_t plan; + float cx, cy; if (disc2 > 0.0f) { const float t2 = w2u + fast_fsqrtf(disc2); - arcR = r2; - arcCx = b0x + t2 * u2x; - arcCy = b0y + t2 * u2y; - phiNomCd = fwBankForRadiusCd(v, r2); - tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiNomCd)); - } else { // drifted beyond the line: keep the planned circle - arcR = intoR; - arcCx = intoO2x; - arcCy = intoO2y; + plan.r = r2; + cx = b0x + t2 * u2x; + cy = b0y + t2 * u2y; + plan.phiNomCd = fwBankForRadiusCd(v, r2); + plan.tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(plan.phiNomCd)); + } else { // drifted beyond the line: keep the planned circle and its bank + plan.r = intoR; + cx = intoO2x; + cy = intoO2y; + plan.phiNomCd = phiNomCd; + plan.tEaseMs = tEaseMs; } - arcOutBearing = intoBOut; + fwArcEngageRampIn(&plan, intoDir, cx, cy, intoBOut, blendFromCd); intoStage = FW_INTO_MAIN; if (turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { /* Committed onto the outbound leg: mark the WP reached (as FLY_BY does at turn start). @@ -1093,17 +1114,9 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) break; } case ARC_STEADY: { - const float dx = pos->x - arcCx; - const float dy = pos->y - arcCy; - const float eR = calc_length_pythagorean_2D(dx, dy) - arcR; // [cm], + = outside the arc - const float alpha = atan2_approx(dy, dx); // azimuth on the arc - float tgx, tgy; - fwTangentDir(alpha, arcDir, &tgx, &tgy); - const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(tgy, tgx)))); - const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent - // FF from current groundspeed: wind changes v along the arc - const float phiLiveCd = fwBankForRadiusCd(v, arcR); - const float steadyCd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + float eR; + int32_t eH; + const float steadyCd = fwArcLawCd(pos->x, pos->y, arcCx, arcCy, arcR, arcDir, cog, v, &eR, &eH); steadyMs += dtMs; const float q = (tEaseMs > 1.0f) ? constrainf(steadyMs / tEaseMs, 0.0f, 1.0f) : 1.0f; // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre @@ -1167,17 +1180,13 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) } const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; - const float dcx = pos->x - posControl.desiredState.pos.x; - const float dcy = pos->y - posControl.desiredState.pos.y; - const float dist = calc_length_pythagorean_2D(dcx, dcy); const float arcRadius = MAX(fwActiveLoiterRadius, (float)NAV_FW_TURN_RADIUS_MIN); const int8_t dir = loiterDirection(); - const float alpha = atan2_approx(dcy, dcx); - float tgx, tgy; - fwTangentDir(alpha, dir, &tgx, &tgy); - const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(tgy, tgx)))); - const int32_t eH = wrap_18000(tangentBearing - posControl.actualState.cog); - const float eR = dist - arcRadius; + const float v = posControl.actualState.velXY; + float eR; + int32_t eH; + const float targetCd = fwArcLawCd(pos->x, pos->y, posControl.desiredState.pos.x, posControl.desiredState.pos.y, + arcRadius, dir, posControl.actualState.cog, v, &eR, &eH); if (!established) { if (fabsf(eR) < NAV_FW_LOITER_CAPTURE_BAND * arcRadius && ABS(eH) < NAV_FW_LOITER_CAPTURE_ALIGN_CD) { @@ -1192,9 +1201,7 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) return; } - const float v = posControl.actualState.velXY; const float phiLiveCd = fwBankForRadiusCd(v, arcRadius); - const float targetCd = dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); const float maxStepCd = fwArcMaxStepCd(phiLiveCd, tEase, US2S(deltaMicros) * 1000.0f); fwArcBankCmd = fwSlewToward(fwArcBankCmd, targetCd, maxStepCd); From 5cbf6e7ca65034b9c31a298d224becdf928a85e1 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 18:20:03 +0200 Subject: [PATCH 4/8] FW nav: keep the turn predictor's state in one struct Thirty-odd file-scope and function-local statics of the arc coordinator, S sequencer, hand-back fade, turn feed-forward, loiter stabiliser and energy guard become fwTurnState_t (148 B) and fwBankGuardState_t (64 B), so the feature's RAM is one sizeof each, and the controller reset's predictor half is one function with exactly the previous reset set. Renames only; a reverse-applied rename map diffs clean against the previous revision. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_fixedwing.c | 564 +++++++++++---------- 1 file changed, 294 insertions(+), 270 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index b0cbf1878df..6eff642af01 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -123,22 +123,78 @@ static bool fwRollSmoothReseed = false; // re-sync the roll S-curve smoother static float fwRollSmoothSeedCd = 0.0f; // baseline the smoother re-seeds to (set by the controller reset) static float fwLastNavRollCmdCd = 0.0f; // last applied nav roll command [centideg] + timestamp, to tell a static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from a pilot handover at reset time -static float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised -static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the loiter circle controller -static bool fwArcActive = false; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) -static bool fwArcEngaged = false; // arc coordinator latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption -static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) -static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc -static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive -static float fwArcHandbackCmdCd = 0.0f; // arc command at release; the PID/FF command is faded in from it so the -static float fwArcHandbackMs = 0.0f; // seam is continuous regardless of nav_fw_control_smoothness (0 = no fade) -static float fwArcHandbackDurMs = 0.0f; -static float fwArcEaseMs = 0.0f; // ease time of the arc in progress, sizes the handback fade -static bool fwArcWasActive = false; // arc drove the roll last frame, to catch the release edge -static float fwTurnFFCmdCd = 0.0f; // slew-limited turn feed-forward command [centideg] -static bool fwTurnFFArmed = false; // FF assists the turn a leg change begins, not later tracking corrections -static int32_t fwTurnFFPrevLegBearing = -1; // last leg bearing seen by the FF arming logic (-1 = unseeded) -static float fwArcPickupAlong = 0.0f; // along-track distance to the second-arc pickup [cm], for the log + +/* The turn predictor's entire static state, so its RAM cost is one sizeof per struct + * (see docs/development/ram-and-flash-optimization.md). */ +typedef struct { + struct { + float cx, cy, r; + float phiNomCd; // coordinated nominal bank for this turn [centideg] + float tEaseMs; // roll-in ease time + float rampMs; // elapsed time in the ramp-in phase + float rampStartCd; // bank the ramp blends from (the live nav command on entry - a banked + // loiter exit must not level off first; the pickup blends mid-arc) + float steadyMs; // elapsed time in the steady phase + float steadyStartCd; // bank the steady law blends from (the ramp's final command) + float bankCmd; // direct-radius arc bank command [centideg] (Approach B), applied to roll while active + float easeMs; // ease time of the arc in progress, sizes the handback fade + float pickupAlong; // along-track distance to the second-arc pickup [cm], for the log + int32_t outBearing; + int32_t prevLegBearing; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) + uint8_t phase; + int8_t dir; + bool toLegLine; // fallback capture: converge onto the leg line itself, not just its course + bool active; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) + bool engaged; // arc latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption + bool wasActive; // arc drove the roll last frame, to catch the release edge + bool flyByCappedLatch; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc + } arc; + struct { // S sequencer: first arc, roll-reversal gap, second arc + float ex, ey; // second-arc pickup point (internal-tangent touch) + float o2x, o2y; // second-arc centre + float r; + float awayMs; // time spent in the away arc, to bound a pickup that never triggers + int32_t bOut; + uint8_t stage; + int8_t dir; + } s; + struct { + float cmdCd; // arc command at release; the PID/FF command is faded in from it so the + float ms; // seam is continuous regardless of nav_fw_control_smoothness (0 = no fade) + float durMs; + } handback; + struct { + float cmdCd; // slew-limited turn feed-forward command [centideg] + int32_t prevLegBearing; // last leg bearing seen by the FF arming logic (-1 = unseeded) + bool armed; // FF assists the turn a leg change begins, not later tracking corrections + } ff; + struct { + float activeRadius; // effective loiter radius in use (cm), for the loiter circle controller + float commandedHold; + float decayTarget; + float revPeak; + float netAngle; + float prevBearing; + bool ratchetActive; + bool established; + } loiter; +} fwTurnState_t; + +typedef struct { + float effectiveBankLimit; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised + float targetVzBaseline; + timeUs_t lastUpdateUs; + timeUs_t lastTriggerUs; + pt1Filter_t deficitFilter; + pt1Filter_t riseFilter; + bool deficitLatched; + bool bankedPrev; +} fwBankGuardState_t; + +// Zero-initialised (.bss): the -1 leg-bearing sentinels come from the controller reset, which +// always runs before the first tick (the position-update timer starts at 0) +static fwTurnState_t fwTurn; +static fwBankGuardState_t fwGuard; static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -335,6 +391,24 @@ bool adjustFixedWingHeadingFromRCInput(void) static fpVector3_t virtualDesiredPosition; static pt1Filter_t fwCrossTrackErrorRateFilterState; +/* The controller reset's turn-predictor half. Deliberately partial: the S stage, the loiter + * ratchet/capture state and the guard's filters and latch survive a reset, and so do the arc + * geometry and command, which a still-established loiter circle keeps slewing from. */ +static void fwTurnStateReset(void) +{ + fwTurn.arc.active = false; + fwTurn.arc.engaged = false; + fwTurn.arc.wasActive = false; + fwTurn.arc.prevLegBearing = -1; + fwTurn.arc.flyByCappedLatch = false; + fwTurn.handback.durMs = 0.0f; + fwTurn.ff.cmdCd = 0.0f; + fwTurn.ff.armed = false; + fwTurn.ff.prevLegBearing = -1; + // 0 = use the full ceiling until the guard re-syncs on its next run + fwGuard.effectiveBankLimit = 0.0f; +} + /* * TODO Currently this function resets both FixedWing and Rover & Boat position controller */ @@ -343,15 +417,7 @@ void resetFixedWingPositionController(void) virtualDesiredPosition.x = 0; virtualDesiredPosition.y = 0; virtualDesiredPosition.z = 0; - fwArcActive = false; - fwArcEngaged = false; - fwArcWasActive = false; - fwArcHandbackDurMs = 0.0f; - fwTurnFFCmdCd = 0.0f; - fwTurnFFArmed = false; - fwTurnFFPrevLegBearing = -1; - fwArcPrevLegBearing = -1; - fwFlyByCappedLatch = false; + fwTurnStateReset(); navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_heading); @@ -366,9 +432,6 @@ void resetFixedWingPositionController(void) fwRollSmoothSeedCd = ((micros() - fwLastNavRollCmdTimeUs) < MAX_POSITION_UPDATE_INTERVAL_US) ? fwLastNavRollCmdCd : 0.0f; fwRollSmoothReseed = true; - // Reset the energy-guard bank limit; 0 = use full ceiling until the guard re-syncs on its next run - fwEffectiveBankLimit = 0.0f; - pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, NAV_FW_CROSSTRACK_RATE_CUTOFF_HZ); pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); } @@ -471,7 +534,7 @@ static float getFwBankCeilingDeg(void) static float getFwEffectiveBankLimit(void) { const float ceiling = getFwBankCeilingDeg(); - return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, ceiling) : ceiling; + return (fwGuard.effectiveBankLimit > 0.0f) ? MIN(fwGuard.effectiveBankLimit, ceiling) : ceiling; } // Planning bank [deg] for sizing turn/loiter radii: the target, capped by the guard ceiling @@ -483,37 +546,29 @@ static float getFwPlanningBankDeg(void) // Roll-command bank limit [deg]: an active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target static float getFwControlBankLimit(void) { - return fwArcActive ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); + return fwTurn.arc.active ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } // Reduce the bank ceiling when a commanded climb stalls near the pitch/throttle limit while banked, so the turn widens and the climb recovers static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottleValue) { - static timeUs_t lastUpdateUs = 0; - static timeUs_t lastTriggerUs = 0; - static bool deficitLatched = false; - static bool bankedPrev = false; - static float targetVzBaseline = 0.0f; - static pt1Filter_t deficitFilter; - static pt1Filter_t riseFilter; - const float maxBank = getFwBankCeilingDeg(); const float targetBank = MIN((float)navConfig()->fw.max_bank_angle, maxBank); // planning target = guard snap level - if (fwEffectiveBankLimit <= 0.0f) { - fwEffectiveBankLimit = maxBank; + if (fwGuard.effectiveBankLimit <= 0.0f) { + fwGuard.effectiveBankLimit = maxBank; } - const timeDeltaLarge_t dtUs = currentTimeUs - lastUpdateUs; - lastUpdateUs = currentTimeUs; + const timeDeltaLarge_t dtUs = currentTimeUs - fwGuard.lastUpdateUs; + fwGuard.lastUpdateUs = currentTimeUs; // First call / gap (controller was inactive): resync, skip integration this step. if (dtUs <= 0 || dtUs > MAX_POSITION_UPDATE_INTERVAL_US) { - targetVzBaseline = posControl.desiredState.vel.z; - pt1FilterSetCutoff(&deficitFilter, NAV_FW_GUARD_VZ_FILTER_HZ); - pt1FilterSetCutoff(&riseFilter, NAV_FW_GUARD_VZ_FILTER_HZ); - pt1FilterReset(&deficitFilter, 0.0f); - pt1FilterReset(&riseFilter, 0.0f); - deficitLatched = false; - lastTriggerUs = currentTimeUs; + fwGuard.targetVzBaseline = posControl.desiredState.vel.z; + pt1FilterSetCutoff(&fwGuard.deficitFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterSetCutoff(&fwGuard.riseFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterReset(&fwGuard.deficitFilter, 0.0f); + pt1FilterReset(&fwGuard.riseFilter, 0.0f); + fwGuard.deficitLatched = false; + fwGuard.lastTriggerUs = currentTimeUs; return; } const float dtSec = US2S(dtUs); @@ -522,23 +577,23 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl const bool banked = bankDeg > NAV_FW_GUARD_MIN_BANK_DEG; // Latch target Vz at bank entry as the pre-bank reference. - if (banked && !bankedPrev) { - targetVzBaseline = posControl.desiredState.vel.z; + if (banked && !fwGuard.bankedPrev) { + fwGuard.targetVzBaseline = posControl.desiredState.vel.z; } - bankedPrev = banked; + fwGuard.bankedPrev = banked; const float targetVz = posControl.desiredState.vel.z; // cm/s const float actualVz = navGetCurrentActualPositionAndVelocity()->vel.z; // cm/s // Signal A: unmet climb demand. Signal B: bank-induced rise of the demand since bank entry. - const float deficit = pt1FilterApply3(&deficitFilter, targetVz - actualVz, dtSec); - const float rise = pt1FilterApply3(&riseFilter, targetVz - targetVzBaseline, dtSec); + const float deficit = pt1FilterApply3(&fwGuard.deficitFilter, targetVz - actualVz, dtSec); + const float rise = pt1FilterApply3(&fwGuard.riseFilter, targetVz - fwGuard.targetVzBaseline, dtSec); // Schmitt trigger + deadband on the filtered deficit (rejects fluctuations). if (deficit > NAV_FW_GUARD_VZ_DEFICIT_ENTER) { - deficitLatched = true; + fwGuard.deficitLatched = true; } else if (deficit < NAV_FW_GUARD_VZ_DEFICIT_EXIT) { - deficitLatched = false; + fwGuard.deficitLatched = false; } const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES((float)navConfig()->fw.max_climb_angle); @@ -547,21 +602,21 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl const bool nearThrottleLimit = autoThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); const bool climbCommanded = targetVz > NAV_FW_GUARD_VZ_CLIMB_MIN; - const bool trigger = banked && climbCommanded && deficitLatched && (nearPitchLimit || nearThrottleLimit); + const bool trigger = banked && climbCommanded && fwGuard.deficitLatched && (nearPitchLimit || nearThrottleLimit); // Bank-induced rise (signal B) -> react faster. const float reduceRate = (rise > NAV_FW_GUARD_VZ_DEFICIT_ENTER) ? (2.0f * NAV_FW_GUARD_REDUCE_RATE_DPS) : NAV_FW_GUARD_REDUCE_RATE_DPS; if (trigger) { - fwEffectiveBankLimit = MIN(fwEffectiveBankLimit, targetBank); // drop headroom at once: snap to the planning target - fwEffectiveBankLimit -= reduceRate * dtSec; // then keep easing down toward the floor - lastTriggerUs = currentTimeUs; - } else if ((currentTimeUs - lastTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { - fwEffectiveBankLimit += NAV_FW_GUARD_RECOVER_RATE_DPS * dtSec; + fwGuard.effectiveBankLimit = MIN(fwGuard.effectiveBankLimit, targetBank); // drop headroom at once: snap to the planning target + fwGuard.effectiveBankLimit -= reduceRate * dtSec; // then keep easing down toward the floor + fwGuard.lastTriggerUs = currentTimeUs; + } else if ((currentTimeUs - fwGuard.lastTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { + fwGuard.effectiveBankLimit += NAV_FW_GUARD_RECOVER_RATE_DPS * dtSec; } - fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); + fwGuard.effectiveBankLimit = constrainf(fwGuard.effectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); - DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] + DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwGuard.effectiveBankLimit)); // energy-guard bank ceiling [deg] } // Landing approach always flies coordinated FLY_BY turns, whatever mode is configured @@ -593,17 +648,17 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr // leg is a path-tracking correction (cross-track carrot), and the full coordinated bank on top of // the PID turns every leg capture into an S-shaped swing. const int32_t ffLegBearing = posControl.activeWaypoint.bearing; - if (fwTurnFFPrevLegBearing < 0 || ABS(wrap_18000(ffLegBearing - fwTurnFFPrevLegBearing)) > 500) { - fwTurnFFArmed = true; + if (fwTurn.ff.prevLegBearing < 0 || ABS(wrap_18000(ffLegBearing - fwTurn.ff.prevLegBearing)) > 500) { + fwTurn.ff.armed = true; } - fwTurnFFPrevLegBearing = ffLegBearing; + fwTurn.ff.prevLegBearing = ffLegBearing; if (ABS(navHeadingError) <= NAV_FW_FF_HEADING_DEADBAND_CD) { - fwTurnFFArmed = false; + fwTurn.ff.armed = false; } float ffRadius = 0.0f; float ffSign = 0.0f; - if (fwTurnFFArmed && !needToCalculateCircularLoiter && isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { + if (fwTurn.ff.armed && !needToCalculateCircularLoiter && isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { ffRadius = getFwCoordinatedTurnRadius(); // WP turn: dynamic radius, tapered by heading error // Taper from the deadband edge, not from zero: measuring from zero leaves a step at the gate ffSign = (navHeadingError > 0 ? 1.0f : -1.0f) @@ -622,59 +677,52 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr // so the FF is a staircase. Rate limiting spreads the steps without lagging a settled command. const float maxStepCd = NAV_FW_FF_SLEW_FRACTION * (currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f) * 100.0f * US2S(deltaMicros); - fwTurnFFCmdCd = fwSlewToward(fwTurnFFCmdCd, rollFF, maxStepCd); + fwTurn.ff.cmdCd = fwSlewToward(fwTurn.ff.cmdCd, rollFF, maxStepCd); - DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(fwTurnFFCmdCd)); // turn/loiter roll feed-forward [centideg] - return fwTurnFFCmdCd; + DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(fwTurn.ff.cmdCd)); // turn/loiter roll feed-forward [centideg] + return fwTurn.ff.cmdCd; } // Stabilised loiter-radius floor [cm]: the raw requirement swings with wind (v^2) and would make the // tracker thrash - ratchet up instantly, hold the peak one revolution, ease down at <= DECAY static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearingFromCenterRad, bool loiterActive, timeDelta_t deltaMicros) { - static bool active = false; - static float commandedHold = 0.0f; - static float decayTarget = 0.0f; - static float revPeak = 0.0f; - static float netAngle = 0.0f; - static float prevBearing = 0.0f; - const float required = getFwCoordinatedTurnRadius(); if (!loiterActive) { - active = false; - commandedHold = required; // transit / WP turn: track instantaneously + fwTurn.loiter.ratchetActive = false; + fwTurn.loiter.commandedHold = required; // transit / WP turn: track instantaneously } else { - if (!active) { // loiter entry: seed (no decay until the first revolution) - active = true; - commandedHold = required; - decayTarget = NAV_FW_TURN_RADIUS_MAX; - revPeak = required; - netAngle = 0.0f; - prevBearing = bearingFromCenterRad; + if (!fwTurn.loiter.ratchetActive) { // loiter entry: seed (no decay until the first revolution) + fwTurn.loiter.ratchetActive = true; + fwTurn.loiter.commandedHold = required; + fwTurn.loiter.decayTarget = NAV_FW_TURN_RADIUS_MAX; + fwTurn.loiter.revPeak = required; + fwTurn.loiter.netAngle = 0.0f; + fwTurn.loiter.prevBearing = bearingFromCenterRad; } - revPeak = MAX(revPeak, required); - commandedHold = MAX(commandedHold, required); // ratchet up immediately (safety) + fwTurn.loiter.revPeak = MAX(fwTurn.loiter.revPeak, required); + fwTurn.loiter.commandedHold = MAX(fwTurn.loiter.commandedHold, required); // ratchet up immediately (safety) - float dAng = bearingFromCenterRad - prevBearing; + float dAng = bearingFromCenterRad - fwTurn.loiter.prevBearing; if (dAng > M_PIf) dAng -= 2.0f * M_PIf; if (dAng < -M_PIf) dAng += 2.0f * M_PIf; - netAngle += dAng; // signed net rotation about the centre - prevBearing = bearingFromCenterRad; + fwTurn.loiter.netAngle += dAng; // signed net rotation about the centre + fwTurn.loiter.prevBearing = bearingFromCenterRad; - if (fabsf(netAngle) >= 2.0f * M_PIf) { // a full revolution -> this revolution's peak is the decay target - decayTarget = revPeak; - revPeak = required; - netAngle = 0.0f; + if (fabsf(fwTurn.loiter.netAngle) >= 2.0f * M_PIf) { // a full revolution -> this revolution's peak is the decay target + fwTurn.loiter.decayTarget = fwTurn.loiter.revPeak; + fwTurn.loiter.revPeak = required; + fwTurn.loiter.netAngle = 0.0f; } - if (commandedHold > decayTarget) { // ease down gradually, never below the current need - commandedHold -= NAV_FW_LOITER_RADIUS_DECAY * US2S(deltaMicros); - commandedHold = MAX(commandedHold, MAX(decayTarget, required)); + if (fwTurn.loiter.commandedHold > fwTurn.loiter.decayTarget) { // ease down gradually, never below the current need + fwTurn.loiter.commandedHold -= NAV_FW_LOITER_RADIUS_DECAY * US2S(deltaMicros); + fwTurn.loiter.commandedHold = MAX(fwTurn.loiter.commandedHold, MAX(fwTurn.loiter.decayTarget, required)); } } - const uint32_t out = (uint32_t)MAX((float)configuredRadius, commandedHold); + const uint32_t out = (uint32_t)MAX((float)configuredRadius, fwTurn.loiter.commandedHold); DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(out)); // active turn/loiter radius [cm] (overridden by FLY_BY/arc writers) return out; } @@ -693,18 +741,18 @@ static float fwTurnEaseTimeMs(float phiNomDeg) // still see the full carrot error, so adopting their command directly is a step of several degrees. static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) { - if (fwArcHandbackDurMs <= 0.0f) { + if (fwTurn.handback.durMs <= 0.0f) { return rollTargetCd; } - fwArcHandbackMs += US2S(deltaMicros) * 1000.0f; - const float p = constrainf(fwArcHandbackMs / fwArcHandbackDurMs, 0.0f, 1.0f); + fwTurn.handback.ms += US2S(deltaMicros) * 1000.0f; + const float p = constrainf(fwTurn.handback.ms / fwTurn.handback.durMs, 0.0f, 1.0f); if (p >= 1.0f) { - fwArcHandbackDurMs = 0.0f; + fwTurn.handback.durMs = 0.0f; return rollTargetCd; } - return fwSmoothBlend(fwArcHandbackCmdCd, rollTargetCd, p); + return fwSmoothBlend(fwTurn.handback.cmdCd, rollTargetCd, p); } // Clamp to the flyable ceiling: rate limits, handoff checks and the smoother seed must not @@ -712,7 +760,7 @@ static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) static void fwArcClampBankCmd(void) { const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); - fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + fwTurn.arc.bankCmd = constrainf(fwTurn.arc.bankCmd, -cmdLimitCd, cmdLimitCd); } // Clear on release, else the log keeps showing the last arc phase for minutes @@ -722,31 +770,9 @@ static void fwArcDebugRelease(void) DEBUG_SET(DEBUG_FW_TURN, 3, 0); } -// Arc coordinator state. At file scope only so the shared transition helpers below can write it; -// nothing outside updateFwTurnArc and those helpers touches any of it. enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; -static uint8_t phase; -static float arcCx, arcCy, arcR; -static int8_t arcDir; -static int32_t arcOutBearing; -static bool arcToLegLine; // fallback capture: converge onto the leg line itself, not just its course -static float phiNomCd; // coordinated nominal bank for this turn [centideg] -static float tEaseMs; // roll-in ease time -static float rampMs; // elapsed time in the ramp-in phase -static float rampStartCd; // bank the ramp blends from (the live nav command on entry - a banked - // loiter exit must not level off first; the pickup blends mid-arc) -static float steadyMs; // elapsed time in the steady phase -static float steadyStartCd; // bank the steady law blends from (the ramp's final command) - // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, roll-reversal gap, second arc enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; -static uint8_t intoStage; -static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) -static float intoO2x, intoO2y; // second-arc centre -static float intoR; -static int32_t intoBOut; -static int8_t intoDir; -static float intoAwayMs; // time spent in the away arc, to bound a pickup that never triggers // Radius, coordinated bank and roll ease time of a planned turn - never meaningful apart typedef struct { @@ -766,33 +792,33 @@ static void fwPlanTurn(float v, fwTurnPlan_t *plan) static void fwArcEngageRampIn(const fwTurnPlan_t *plan, int8_t dir, float cx, float cy, int32_t outBearing, float blendFromCd) { - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = blendFromCd; - arcR = plan->r; - arcDir = dir; - arcCx = cx; - arcCy = cy; - arcOutBearing = outBearing; - phiNomCd = plan->phiNomCd; - tEaseMs = plan->tEaseMs; + fwTurn.arc.engaged = true; + fwTurn.arc.phase = ARC_RAMP_IN; + fwTurn.arc.rampMs = 0.0f; + fwTurn.arc.rampStartCd = blendFromCd; + fwTurn.arc.r = plan->r; + fwTurn.arc.dir = dir; + fwTurn.arc.cx = cx; + fwTurn.arc.cy = cy; + fwTurn.arc.outBearing = outBearing; + fwTurn.arc.phiNomCd = plan->phiNomCd; + fwTurn.arc.tEaseMs = plan->tEaseMs; } // No usable circle geometry left: capture the leg line itself with the bounded roll-out static void fwArcCaptureToLeg(int32_t legBearing) { - arcOutBearing = legBearing; - arcToLegLine = true; - phase = ARC_CAPTURE; + fwTurn.arc.outBearing = legBearing; + fwTurn.arc.toLegLine = true; + fwTurn.arc.phase = ARC_CAPTURE; } // Stage the second arc of an S so the shared pickup logic can re-anchor and fly it static void fwArcStageSecondArc(float o2x, float o2y, float nAng, float r, int32_t bOut, int8_t dir) { - fwPolarOffset(o2x, o2y, -r, nAng, &intoEx, &intoEy); // pickup = internal-tangent touch on the circle - intoO2x = o2x; intoO2y = o2y; - intoR = r; intoBOut = bOut; intoDir = dir; + fwPolarOffset(o2x, o2y, -r, nAng, &fwTurn.s.ex, &fwTurn.s.ey); // pickup = internal-tangent touch on the circle + fwTurn.s.o2x = o2x; fwTurn.s.o2y = o2y; + fwTurn.s.r = r; fwTurn.s.bOut = bOut; fwTurn.s.dir = dir; } // Steady arc law: coordinated bank for the live speed (wind changes v along the arc) plus radial @@ -815,18 +841,18 @@ static float fwArcLawCd(float px, float py, float cx, float cy, float r, int8_t } // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive -// capture roll-out. Sets fwArcActive (drives the roll directly). +// capture roll-out. Sets fwTurn.arc.active (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) { - fwArcActive = false; + fwTurn.arc.active = false; const navFwWpTurnMode_e turnMode = fwEffectiveTurnMode(); const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; if (turnMode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { - fwArcEngaged = false; - fwArcPrevLegBearing = -1; - intoStage = FW_INTO_IDLE; + fwTurn.arc.engaged = false; + fwTurn.arc.prevLegBearing = -1; + fwTurn.s.stage = FW_INTO_IDLE; fwArcDebugRelease(); return; } @@ -838,19 +864,19 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float v = posControl.actualState.velXY; const float dtMs = US2S(deltaMicros) * 1000.0f; - if (!fwArcEngaged) { - arcToLegLine = false; + if (!fwTurn.arc.engaged) { + fwTurn.arc.toLegLine = false; /* Unseeded (loiter/reset wipes the reference every cycle) means a WP advance landing on the * first tracked cycle is invisible - the loiter exit then never engages and the PID+FF fly * the whole turn unshaped. Unseeded + grossly off the leg course = a pending turn. */ - const bool legChanged = (fwArcPrevLegBearing >= 0) - ? (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) + const bool legChanged = (fwTurn.arc.prevLegBearing >= 0) + ? (ABS(wrap_18000(legBearing - fwTurn.arc.prevLegBearing)) > 500) : (ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); - fwArcPrevLegBearing = legBearing; + fwTurn.arc.prevLegBearing = legBearing; if (legChanged) { - intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry - const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits - fwFlyByCappedLatch = false; // consume the latch on any leg change + fwTurn.s.stage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry + const bool capped = fwTurn.arc.flyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits + fwTurn.arc.flyByCappedLatch = false; // consume the latch on any leg change if (turnMode == NAV_FW_WP_TURN_COORD_FLY_OVER) { // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. @@ -890,12 +916,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * plan.r) { fwArcStageSecondArc(o2x, o2y, nAng, plan.r, legBearing, -dirTmp); fwArcEngageRampIn(&plan, dirTmp, cx, cy, fwRadToBearingCd(icptRad), fwLastNavRollCmdCd); - intoStage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over + fwTurn.s.stage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over } } } const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); - if (!fwArcEngaged && dCP > 1.05f * plan.r) { // next WP outside the circle: a tangent exists + if (!fwTurn.arc.engaged && dCP > 1.05f * plan.r) { // next WP outside the circle: a tangent exists const float alphaCP = atan2_approx(py - cy, px - cx); const float phiT = acos_approx(constrainf(plan.r / dCP, 0.0f, 1.0f)); for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP @@ -918,14 +944,14 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float psiTmp = 0.5f * omegaNomCds * (plan.tEaseMs / 1000.0f); if (capped || ABS(hdgErrToLeg) > NAV_FW_ARC_SHARP_TURN_CD) { // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly - fwArcEngaged = true; - fwArcBankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step - rampMs = 0.0f; - rampStartCd = 0.0f; - arcR = plan.r; // capture ignores it, but the away timeout is sized from it - arcDir = (hdgErrToLeg > 0) ? 1 : -1; - phiNomCd = plan.phiNomCd; - tEaseMs = plan.tEaseMs; + fwTurn.arc.engaged = true; + fwTurn.arc.bankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step + fwTurn.arc.rampMs = 0.0f; + fwTurn.arc.rampStartCd = 0.0f; + fwTurn.arc.r = plan.r; // capture ignores it, but the away timeout is sized from it + fwTurn.arc.dir = (hdgErrToLeg > 0) ? 1 : -1; + fwTurn.arc.phiNomCd = plan.phiNomCd; + fwTurn.arc.tEaseMs = plan.tEaseMs; fwArcCaptureToLeg(legBearing); } else if (2.0f * psiTmp < (float)ABS(hdgErrToLeg)) { // enough turn left for a steady arc between the ease ramps const int8_t dirTmp = (hdgErrToLeg > 0) ? 1 : -1; @@ -947,12 +973,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - if (!fwArcEngaged && turnMode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { - if (intoStage == FW_INTO_MAIN) { + if (!fwTurn.arc.engaged && turnMode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (fwTurn.s.stage == FW_INTO_MAIN) { // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) - intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; + fwTurn.s.stage = (ABS(wrap_18000(legBearing - fwTurn.s.bOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; } - if (intoStage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { + if (fwTurn.s.stage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { fwTurnPlan_t plan; @@ -987,64 +1013,64 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // counter-arc first, rolling out onto the internal-tangent course fwArcEngageRampIn(&plan, -dirM, o1x, o1y, fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)), fwLastNavRollCmdCd); - intoStage = FW_INTO_AWAY; + fwTurn.s.stage = FW_INTO_AWAY; } } } - } else if (intoStage == FW_INTO_AWAY) { - intoStage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale + } else if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.stage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale } } - if (!fwArcEngaged) { - DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); + if (!fwTurn.arc.engaged) { + DEBUG_SET(DEBUG_FW_TURN, 1, fwTurn.s.stage); return; } } else { // Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale // out-bearing - unless the arc already flies toward that leg (the FLY_INTO commit above advances // the WP mid-main-arc by design; retargeting would degrade the shaped arc to a bare capture) - if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500 - && ABS(wrap_18000(legBearing - arcOutBearing)) > 500) { - fwFlyByCappedLatch = false; + if (ABS(wrap_18000(legBearing - fwTurn.arc.prevLegBearing)) > 500 + && ABS(wrap_18000(legBearing - fwTurn.arc.outBearing)) > 500) { + fwTurn.arc.flyByCappedLatch = false; fwArcCaptureToLeg(legBearing); - if (intoStage == FW_INTO_AWAY) { - intoStage = FW_INTO_DONE; // staged S is stale: release the hand-back block + if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.stage = FW_INTO_DONE; // staged S is stale: release the hand-back block } } - fwArcPrevLegBearing = legBearing; + fwTurn.arc.prevLegBearing = legBearing; // Pick up the second arc at its tangency point. Along-track distance so a lateral residual // cannot miss it; the heading gate blocks the trigger early in the first arc, where the // pickup point still lies behind the exit course. float outUx, outUy; - fwBearingUnit(arcOutBearing, &outUx, &outUy); - fwArcPickupAlong = (intoEx - pos->x) * outUx + (intoEy - pos->y) * outUy; + fwBearingUnit(fwTurn.arc.outBearing, &outUx, &outUy); + fwTurn.arc.pickupAlong = (fwTurn.s.ex - pos->x) * outUx + (fwTurn.s.ey - pos->y) * outUy; // The away arc blocks the hand-back, so a pickup that never triggers strands the aircraft with // path tracking suppressed. Bound it by the time to fly half the away circle. - if (intoStage == FW_INTO_AWAY) { - intoAwayMs += dtMs; - if (intoAwayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * arcR / MAX(v, NAV_FW_TURN_MIN_SPEED)) { - intoStage = FW_INTO_DONE; // release the block; the capture finishes on the leg + if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.awayMs += dtMs; + if (fwTurn.s.awayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * fwTurn.arc.r / MAX(v, NAV_FW_TURN_MIN_SPEED)) { + fwTurn.s.stage = FW_INTO_DONE; // release the block; the capture finishes on the leg fwArcCaptureToLeg(legBearing); } } else { - intoAwayMs = 0.0f; + fwTurn.s.awayMs = 0.0f; } - if (intoStage == FW_INTO_AWAY - && ABS(wrap_18000(arcOutBearing - cog)) < 4500 - && fwArcPickupAlong <= fwRollInLeadCm(v, tEaseMs)) { - const float blendFromCd = fwArcBankCmd; // blend from the bank flown right now + if (fwTurn.s.stage == FW_INTO_AWAY + && ABS(wrap_18000(fwTurn.arc.outBearing - cog)) < 4500 + && fwTurn.arc.pickupAlong <= fwRollInLeadCm(v, fwTurn.arc.tEaseMs)) { + const float blendFromCd = fwTurn.arc.bankCmd; // blend from the bank flown right now // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the // circle re-anchored along the leg line through the actual position: wind drift becomes // an along-track shift instead of a parallel roll-out offset const float r2 = getFwCoordinatedTurnRadius(); - const float legR2 = CENTIDEGREES_TO_RADIANS((float)intoBOut); + const float legR2 = CENTIDEGREES_TO_RADIANS((float)fwTurn.s.bOut); float u2x, u2y, b0x, b0y; - fwBearingUnit(intoBOut, &u2x, &u2y); + fwBearingUnit(fwTurn.s.bOut, &u2x, &u2y); fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - r2, legR2, (float)intoDir, &b0x, &b0y); + r2, legR2, (float)fwTurn.s.dir, &b0x, &b0y); const float w2x = pos->x - b0x, w2y = pos->y - b0y; const float w2u = w2x * u2x + w2y * u2y; const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; @@ -1058,14 +1084,14 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) plan.phiNomCd = fwBankForRadiusCd(v, r2); plan.tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(plan.phiNomCd)); } else { // drifted beyond the line: keep the planned circle and its bank - plan.r = intoR; - cx = intoO2x; - cy = intoO2y; - plan.phiNomCd = phiNomCd; - plan.tEaseMs = tEaseMs; + plan.r = fwTurn.s.r; + cx = fwTurn.s.o2x; + cy = fwTurn.s.o2y; + plan.phiNomCd = fwTurn.arc.phiNomCd; + plan.tEaseMs = fwTurn.arc.tEaseMs; } - fwArcEngageRampIn(&plan, intoDir, cx, cy, intoBOut, blendFromCd); - intoStage = FW_INTO_MAIN; + fwArcEngageRampIn(&plan, fwTurn.s.dir, cx, cy, fwTurn.s.bOut, blendFromCd); + fwTurn.s.stage = FW_INTO_MAIN; if (turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { /* Committed onto the outbound leg: mark the WP reached (as FLY_BY does at turn start). * The corner cut never crosses the passage plane through the WP, so no geometric check @@ -1078,54 +1104,54 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - // a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with // the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. - if (arcToLegLine && navConfig()->fw.wp_tracking_accuracy) { + if (fwTurn.arc.toLegLine && navConfig()->fw.wp_tracking_accuracy) { float legUx, legUy; fwBearingUnit(legBearing, &legUx, &legUy); const float offLeg = fwOffLegCm(pos->x, pos->y, posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, legUx, legUy); const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); - arcOutBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); + fwTurn.arc.outBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); } - rampMs += dtMs; - fwArcEaseMs = tEaseMs; // published for the handback fade - const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); + fwTurn.arc.rampMs += dtMs; + fwTurn.arc.easeMs = fwTurn.arc.tEaseMs; // published for the handback fade + const int32_t hdgErrOut = wrap_18000(fwTurn.arc.outBearing - cog); // Roll-out lead: heading consumed by the shaped down-ramp plus the angle-P tail and servo delay const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; - const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (tEaseMs / 1000.0f); - const float maxStepCd = fwArcMaxStepCd(phiNomCd, tEaseMs, dtMs); + const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (fwTurn.arc.tEaseMs / 1000.0f); + const float maxStepCd = fwArcMaxStepCd(fwTurn.arc.phiNomCd, fwTurn.arc.tEaseMs, dtMs); - switch (phase) { + switch (fwTurn.arc.phase) { case ARC_RAMP_IN: { // Rate-consistent: a swing spanning 2*phi takes twice the standard ease time - const float rampSpanCd = fabsf((float)arcDir * phiNomCd - rampStartCd); - const float rampDurMs = MAX(tEaseMs * rampSpanCd / MAX(phiNomCd, 1.0f), 0.5f * tEaseMs); - const float p = (rampDurMs > 1.0f) ? constrainf(rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; - fwArcBankCmd = fwSmoothBlend(rampStartCd, (float)arcDir * phiNomCd, p); // smoothstep up + const float rampSpanCd = fabsf((float)fwTurn.arc.dir * fwTurn.arc.phiNomCd - fwTurn.arc.rampStartCd); + const float rampDurMs = MAX(fwTurn.arc.tEaseMs * rampSpanCd / MAX(fwTurn.arc.phiNomCd, 1.0f), 0.5f * fwTurn.arc.tEaseMs); + const float p = (rampDurMs > 1.0f) ? constrainf(fwTurn.arc.rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; + fwTurn.arc.bankCmd = fwSmoothBlend(fwTurn.arc.rampStartCd, (float)fwTurn.arc.dir * fwTurn.arc.phiNomCd, p); // smoothstep up if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle - phase = ARC_STEADY; - steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc - steadyStartCd = fwArcBankCmd; // law starts displaced: blend into it, don't step + fwTurn.arc.phase = ARC_STEADY; + fwTurn.arc.steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc + fwTurn.arc.steadyStartCd = fwTurn.arc.bankCmd; // law starts displaced: blend into it, don't step } break; } case ARC_STEADY: { float eR; int32_t eH; - const float steadyCd = fwArcLawCd(pos->x, pos->y, arcCx, arcCy, arcR, arcDir, cog, v, &eR, &eH); - steadyMs += dtMs; - const float q = (tEaseMs > 1.0f) ? constrainf(steadyMs / tEaseMs, 0.0f, 1.0f) : 1.0f; + const float steadyCd = fwArcLawCd(pos->x, pos->y, fwTurn.arc.cx, fwTurn.arc.cy, fwTurn.arc.r, fwTurn.arc.dir, cog, v, &eR, &eH); + fwTurn.arc.steadyMs += dtMs; + const float q = (fwTurn.arc.tEaseMs > 1.0f) ? constrainf(fwTurn.arc.steadyMs / fwTurn.arc.tEaseMs, 0.0f, 1.0f) : 1.0f; // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre // and eH can invert between two updates, which would otherwise go straight to the servos - const float blended = fwSmoothBlend(steadyStartCd, steadyCd, q); - fwArcBankCmd = fwSlewToward(fwArcBankCmd, blended, maxStepCd); - if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) + const float blended = fwSmoothBlend(fwTurn.arc.steadyStartCd, steadyCd, q); + fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, blended, maxStepCd); + if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwTurn.arc.bankCmd) || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it - phase = ARC_CAPTURE; + fwTurn.arc.phase = ARC_CAPTURE; } break; } @@ -1135,16 +1161,16 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // unlimited, which stepped the servos at fresh capped engages and mid-capture mission advances int32_t captureErr = hdgErrOut; if (ABS(captureErr) > 17000) { - captureErr = arcDir * ABS(captureErr); // ambiguous reversal: hold the engagement direction + captureErr = fwTurn.arc.dir * ABS(captureErr); // ambiguous reversal: hold the engagement direction } const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); - const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); - fwArcBankCmd = fwSlewToward(fwArcBankCmd, cmd, maxStepCd); + const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -fwTurn.arc.phiNomCd, fwTurn.arc.phiNomCd); + fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, cmd, maxStepCd); // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and // path tracking a moment of control while we sit a full turn diameter off the leg. - if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= NAV_FW_ARC_EXIT_BANK_CD - && intoStage != FW_INTO_AWAY) { // aligned and nearly level -> hand back - fwArcEngaged = false; + if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwTurn.arc.bankCmd) <= NAV_FW_ARC_EXIT_BANK_CD + && fwTurn.s.stage != FW_INTO_AWAY) { // aligned and nearly level -> hand back + fwTurn.arc.engaged = false; fwArcDebugRelease(); return; } @@ -1155,32 +1181,30 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcClampBankCmd(); if (debugMode == DEBUG_FW_TURN) { - debug[0] = lrintf(arcR); // active arc radius [cm] - debug[1] = (phase + 1) * 10 + intoStage; // coordinator state: (arc phase + 1)*10 + S stage - debug[2] = arcOutBearing; // exit course [centideg] + debug[0] = lrintf(fwTurn.arc.r); // active arc radius [cm] + debug[1] = (fwTurn.arc.phase + 1) * 10 + fwTurn.s.stage; // coordinator state: (arc phase + 1)*10 + S stage + debug[2] = fwTurn.arc.outBearing; // exit course [centideg] debug[3] = hdgErrOut; // remaining heading to the exit course [centideg] - debug[4] = lrintf(fwArcBankCmd); // arc bank command [centideg] - debug[7] = lrintf(tEaseMs); // roll ease time [ms] -> sizes the turn leads - if (intoStage == FW_INTO_AWAY) { - debug[6] = lrintf(fwArcPickupAlong); // away arc: along-track distance to the pickup [cm] + debug[4] = lrintf(fwTurn.arc.bankCmd); // arc bank command [centideg] + debug[7] = lrintf(fwTurn.arc.tEaseMs); // roll ease time [ms] -> sizes the turn leads + if (fwTurn.s.stage == FW_INTO_AWAY) { + debug[6] = lrintf(fwTurn.arc.pickupAlong); // away arc: along-track distance to the pickup [cm] } } - fwArcActive = true; + fwTurn.arc.active = true; } // Loiter circle controller: once established on the hold circle, the steady arc law replaces the // carrot PID - live FF bank plus radial/tangent feedback hold the stabilised radius exactly static void updateFwLoiterArc(timeDelta_t deltaMicros) { - static bool established = false; - - if (!needToCalculateCircularLoiter || fwArcEngaged) { - established = false; + if (!needToCalculateCircularLoiter || fwTurn.arc.engaged) { + fwTurn.loiter.established = false; return; } const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; - const float arcRadius = MAX(fwActiveLoiterRadius, (float)NAV_FW_TURN_RADIUS_MIN); + const float arcRadius = MAX(fwTurn.loiter.activeRadius, (float)NAV_FW_TURN_RADIUS_MIN); const int8_t dir = loiterDirection(); const float v = posControl.actualState.velXY; float eR; @@ -1188,28 +1212,28 @@ static void updateFwLoiterArc(timeDelta_t deltaMicros) const float targetCd = fwArcLawCd(pos->x, pos->y, posControl.desiredState.pos.x, posControl.desiredState.pos.y, arcRadius, dir, posControl.actualState.cog, v, &eR, &eH); - if (!established) { + if (!fwTurn.loiter.established) { if (fabsf(eR) < NAV_FW_LOITER_CAPTURE_BAND * arcRadius && ABS(eH) < NAV_FW_LOITER_CAPTURE_ALIGN_CD) { - established = true; - fwArcBankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step + fwTurn.loiter.established = true; + fwTurn.arc.bankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step } else { DEBUG_SET(DEBUG_FW_TURN, 1, 4); // loiter approach, carrot guidance return; } } else if (fabsf(eR) > NAV_FW_LOITER_RELEASE_BAND * arcRadius) { - established = false; // grossly displaced: hand back to the carrot + fwTurn.loiter.established = false; // grossly displaced: hand back to the carrot return; } const float phiLiveCd = fwBankForRadiusCd(v, arcRadius); const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); const float maxStepCd = fwArcMaxStepCd(phiLiveCd, tEase, US2S(deltaMicros) * 1000.0f); - fwArcBankCmd = fwSlewToward(fwArcBankCmd, targetCd, maxStepCd); + fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, targetCd, maxStepCd); fwArcClampBankCmd(); DEBUG_SET(DEBUG_FW_TURN, 1, 40); // loiter circle controller engaged - DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); - fwArcActive = true; + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwTurn.arc.bankCmd)); + fwTurn.arc.active = true; } static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t deltaMicros) @@ -1233,7 +1257,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t const bool inLoiter = (navGetCurrentStateFlags() & NAV_CTL_HOLD); const float bearingFromCenter = atan2_approx(-posErrorY, -posErrorX); navLoiterRadius = getFwStableLoiterRadius(navLoiterRadius, bearingFromCenter, inLoiter, deltaMicros); - fwActiveLoiterRadius = (float)navLoiterRadius; // expose to the turn feed-forward + fwTurn.loiter.activeRadius = (float)navLoiterRadius; // expose to the turn feed-forward fpVector3_t loiterCenterPos = posControl.desiredState.pos; int8_t loiterTurnDirection = loiterDirection(); @@ -1272,7 +1296,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); // FLY_BY planning radius while approaching if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; - fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead + fwTurn.arc.flyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead } } @@ -1380,7 +1404,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled force craft toward and closely track along waypoint course line. * Suppressed while the arc coordinator drives a turn (it tracks the arc, not the straight leg). */ - if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter && !fwArcActive) { + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter && !fwTurn.arc.active) { if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { @@ -1459,7 +1483,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Freeze the integrator while the arc drives the turn - the carrot error keeps one sign and winds it up const pidControllerFlags_e pidFlags = PID_USING_HEADING | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0) - | (fwArcActive ? PID_FREEZE_INTEGRATOR : 0); + | (fwTurn.arc.active ? PID_FREEZE_INTEGRATOR : 0); // Input error in (deg*100), output roll angle (deg*100) const float navBankLimit = getFwControlBankLimit(); // planning target on WP turns, guard ceiling in loiter @@ -1471,23 +1495,23 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Arc bank overrides the PID; its ramps are already shaped, so the S-curve smoother is bypassed // and re-seeded at handback to avoid a stale-state step - if (fwArcActive) { - rollAdjustment = fwArcBankCmd; - fwRollSmoothSeedCd = fwArcBankCmd; // else the handback re-seeds from a stale reset value (brief roll twitch) + if (fwTurn.arc.active) { + rollAdjustment = fwTurn.arc.bankCmd; + fwRollSmoothSeedCd = fwTurn.arc.bankCmd; // else the handback re-seeds from a stale reset value (brief roll twitch) fwRollSmoothReseed = true; - fwArcHandbackDurMs = 0.0f; // an arc that re-engages cancels a fade still in progress - fwArcWasActive = true; + fwTurn.handback.durMs = 0.0f; // an arc that re-engages cancels a fade still in progress + fwTurn.arc.wasActive = true; // The FF is not called while the arc drives: clear and disarm it, and consume leg changes the // arc handles itself - the carrot error left at hand-back is a capture correction, not a turn - fwTurnFFCmdCd = 0.0f; - fwTurnFFArmed = false; - fwTurnFFPrevLegBearing = posControl.activeWaypoint.bearing; + fwTurn.ff.cmdCd = 0.0f; + fwTurn.ff.armed = false; + fwTurn.ff.prevLegBearing = posControl.activeWaypoint.bearing; } else { - if (fwArcWasActive) { // falling edge: arm the crossfade from the arc's last command - fwArcHandbackCmdCd = fwLastNavRollCmdCd; - fwArcHandbackMs = 0.0f; - fwArcHandbackDurMs = fwArcEaseMs; - fwArcWasActive = false; + if (fwTurn.arc.wasActive) { // falling edge: arm the crossfade from the arc's last command + fwTurn.handback.cmdCd = fwLastNavRollCmdCd; + fwTurn.handback.ms = 0.0f; + fwTurn.handback.durMs = fwTurn.arc.easeMs; + fwTurn.arc.wasActive = false; } // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. rollAdjustment += getFwTurnFeedForward(navHeadingError, deltaMicros); From e4be4b102c74ec3c38f0370c0761a0a55355df43 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 18:40:13 +0200 Subject: [PATCH 5/8] FW nav: split the arc coordinator into planning, sequencer and phase steps updateFwTurnArc nested up to eight levels over 350 lines. Its branches are now named functions - one per turn-mode planner, the two S-sequencer halves, leg-change retarget, leg-line capture, roll-out lead and the three phase steps - fed by one per-tick context. Bodies are extracted verbatim (per-block normalised diff is byte-identical); the only control flow edits are inverted early returns on integer guards. MATEKF722 and MATEKH743 flash unchanged, MATEKF405 +448 B from -O2 inlining layout. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_fixedwing.c | 680 ++++++++++++--------- 1 file changed, 390 insertions(+), 290 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 6eff642af01..1e8fcb1bd92 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -840,6 +840,367 @@ static float fwArcLawCd(float px, float py, float cx, float cy, float r, int8_t return dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; } +static void fwArcDisengageIdle(void) +{ + fwTurn.arc.engaged = false; + fwTurn.arc.prevLegBearing = -1; + fwTurn.s.stage = FW_INTO_IDLE; + fwArcDebugRelease(); +} + +// Per-tick inputs of the turn coordinator, read once at the top of the tick +typedef struct { + int32_t legBearing; + int32_t cog; + const fpVector3_t *pos; + float v; + float dtMs; + navFwWpTurnMode_e turnMode; + int32_t hdgErrToLeg; + float lastNavRollCmdCd; // bank an engage blends from; written only after this controller runs +} fwArcCtx_t; + +// Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the +// leg, gamma = half the turn for shallow corners), short straight for the reverse +// roll, then a standard corner-cut arc rolls out ON the line +static void fwArcPlanFlyOverTrackingS(const fwArcCtx_t *c, const fwTurnPlan_t *plan, int8_t dirTmp, + float cx, float cy, float px, float py) +{ + const float legRad = CENTIDEGREES_TO_RADIANS((float)c->legBearing); + float ux, uy; + fwBearingUnit(c->legBearing, &ux, &uy); + const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(c->hdgErrToLeg), 2000.0f, 4500.0f)); + const float icptRad = legRad + (float)dirTmp * gammaRad; + const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); + const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); + float p1x, p1y, b0x, b0y, o2x, o2y; + fwPolarOffset(cx, cy, 2.0f * plan->r, nAng, &p1x, &p1y); // intercept line shifted R toward the counter side + fwPerpOffset(px, py, plan->r, legRad, -(float)dirTmp, &b0x, &b0y); + // gamma >= 20 deg keeps the lines well separated + if (fwLineIntersect(p1x, p1y, d1x, d1y, b0x, b0y, ux, uy, 0.17f, &o2x, &o2y)) { + const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; + const float legLen = calc_length_pythagorean_2D(px - c->pos->x, py - c->pos->y); + // roll-out tangency must lie ahead of us and leave straight leg to the WP + if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * plan->r) { + fwArcStageSecondArc(o2x, o2y, nAng, plan->r, c->legBearing, -dirTmp); + fwArcEngageRampIn(plan, dirTmp, cx, cy, fwRadToBearingCd(icptRad), c->lastNavRollCmdCd); + fwTurn.s.stage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over + } + } +} + +// Tracking OFF fallback: leave the pinned circle on the tangent that points at the next WP +static void fwArcPlanTangentExit(const fwArcCtx_t *c, const fwTurnPlan_t *plan, int8_t dirTmp, + float cx, float cy, float px, float py) +{ + const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); + if (!fwTurn.arc.engaged && dCP > 1.05f * plan->r) { // next WP outside the circle: a tangent exists + const float alphaCP = atan2_approx(py - cy, px - cx); + const float phiT = acos_approx(constrainf(plan->r / dCP, 0.0f, 1.0f)); + for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP + const float th = alphaCP + (float)s * phiT; + float tx, ty, tdx, tdy; + fwPolarOffset(cx, cy, plan->r, th, &tx, &ty); + fwTangentDir(th, dirTmp, &tdx, &tdy); + if ((px - tx) * tdx + (py - ty) * tdy > 0.0f) { + fwArcEngageRampIn(plan, dirTmp, cx, cy, + fwRadToBearingCd(atan2_approx(py - ty, px - tx)), c->lastNavRollCmdCd); + } + } + } +} + +// FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent +// through the next WP; tracking ON: bounded-intercept S onto the new leg itself. +static void fwArcPlanFlyOver(const fwArcCtx_t *c) +{ + const float px = posControl.activeWaypoint.pos.x; + const float py = posControl.activeWaypoint.pos.y; + const int32_t brgToWp = fwRadToBearingCd(atan2_approx(py - c->pos->y, px - c->pos->x)); + const int32_t toWpErr = wrap_18000(brgToWp - c->cog); + if (ABS(toWpErr) <= NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + return; + } + + fwTurnPlan_t plan; + fwPlanTurn(c->v, &plan); + const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; + const float cogRad = CENTIDEGREES_TO_RADIANS((float)c->cog); + // Pin ahead by the roll-in drift so the ramp ends ON the circle + const float leadDist = fwRollInLeadCm(c->v, plan.tEaseMs); + float leadX, leadY, cx, cy; + fwPolarOffset(c->pos->x, c->pos->y, leadDist, cogRad, &leadX, &leadY); + fwPerpOffset(leadX, leadY, plan.r, cogRad, dirTmp, &cx, &cy); + if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + fwArcPlanFlyOverTrackingS(c, &plan, dirTmp, cx, cy, px, py); + } + fwArcPlanTangentExit(c, &plan, dirTmp, cx, cy, px, py); +} + +static void fwArcPlanCornerTurn(const fwArcCtx_t *c, bool capped) +{ + fwTurnPlan_t plan; + fwPlanTurn(c->v, &plan); + const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(c->v / plan.r)); // v/R == g*tan(phi)/v + const float psiTmp = 0.5f * omegaNomCds * (plan.tEaseMs / 1000.0f); + if (capped || ABS(c->hdgErrToLeg) > NAV_FW_ARC_SHARP_TURN_CD) { + // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly + fwTurn.arc.engaged = true; + fwTurn.arc.bankCmd = c->lastNavRollCmdCd; // blend from the current command: no engage step + fwTurn.arc.rampMs = 0.0f; + fwTurn.arc.rampStartCd = 0.0f; + fwTurn.arc.r = plan.r; // capture ignores it, but the away timeout is sized from it + fwTurn.arc.dir = (c->hdgErrToLeg > 0) ? 1 : -1; + fwTurn.arc.phiNomCd = plan.phiNomCd; + fwTurn.arc.tEaseMs = plan.tEaseMs; + fwArcCaptureToLeg(c->legBearing); + } else if (2.0f * psiTmp < (float)ABS(c->hdgErrToLeg)) { // enough turn left for a steady arc between the ease ramps + const int8_t dirTmp = (c->hdgErrToLeg > 0) ? 1 : -1; + // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg + const float cogRad = CENTIDEGREES_TO_RADIANS((float)c->cog); + const float legRad = CENTIDEGREES_TO_RADIANS((float)c->legBearing); + float d1x, d1y, d2x, d2y, p1x, p1y, p2x, p2y, cx, cy; + fwBearingUnit(c->cog, &d1x, &d1y); + fwBearingUnit(c->legBearing, &d2x, &d2y); + fwPerpOffset(c->pos->x, c->pos->y, plan.r, cogRad, dirTmp, &p1x, &p1y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + plan.r, legRad, dirTmp, &p2x, &p2y); + // legs not near-parallel (30..160 deg turn); degenerate -> tangent at the entry point + if (!fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, 0.087f, &cx, &cy)) { + cx = p1x; + cy = p1y; + } + fwArcEngageRampIn(&plan, dirTmp, cx, cy, c->legBearing, c->lastNavRollCmdCd); + } +} + +static void fwArcPlanNewLeg(const fwArcCtx_t *c, bool capped) +{ + if (c->turnMode == NAV_FW_WP_TURN_COORD_FLY_OVER) { + fwArcPlanFlyOver(c); + } else if (ABS(c->hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + fwArcPlanCornerTurn(c, capped); + } +} + +// FLY_INTO: stage the S ahead of the WP - counter arc away from the corner, then the main arc +static void fwArcPlanFlyInto(const fwArcCtx_t *c) +{ + const int32_t nta = posControl.activeWaypoint.nextTurnAngle; + if (nta == -1 || ABS(nta) <= NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + return; + } + + fwTurnPlan_t plan; + fwPlanTurn(c->v, &plan); + const int8_t dirM = (nta > 0) ? 1 : -1; + const int32_t bOut = wrap_36000(c->legBearing + nta); + const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); + const float bInRad = CENTIDEGREES_TO_RADIANS((float)c->legBearing); + float ux, uy; + fwBearingUnit(c->legBearing, &ux, &uy); + // Main circle pinned at the WP, counter circle on the inbound leg; centers + // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room + const float Ls = 2.0f * c->v * (plan.tEaseMs / 1000.0f); + float o2x, o2y, ax, ay; + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + plan.r, bOutRad, dirM, &o2x, &o2y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + plan.r, bInRad, -(float)dirM, &ax, &ay); + const float wx = o2x - ax, wy = o2y - ay; + const float wu = wx * ux + wy * uy; + const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * plan.r * plan.r + Ls * Ls; + if (disc > 0.0f) { + const float s = wu - fast_fsqrtf(disc); // signed along-leg offset of the S start from the WP + const float triggerDist = -s + fwRollInLeadCm(c->v, plan.tEaseMs); + if (s < 0.0f && posControl.wpDistance < triggerDist) { + const float o1x = ax + s * ux; + const float o1y = ay + s * uy; + const float cAng = atan2_approx(o2y - o1y, o2x - o1x); + const float beta = atan2_approx(Ls, 2.0f * plan.r); + const float nAng = cAng + (float)dirM * beta; + fwArcStageSecondArc(o2x, o2y, nAng, plan.r, bOut, dirM); + // counter-arc first, rolling out onto the internal-tangent course + fwArcEngageRampIn(&plan, -dirM, o1x, o1y, + fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)), c->lastNavRollCmdCd); + fwTurn.s.stage = FW_INTO_AWAY; + } + } +} + +// S sequencer while no arc is engaged: re-arm after a flown crossing, then stage the next one +static void fwArcSequencerIdle(const fwArcCtx_t *c) +{ + if (fwTurn.arc.engaged || c->turnMode == NAV_FW_WP_TURN_COORD_FLY_BY || !(navGetCurrentStateFlags() & NAV_AUTO_WP)) { + return; + } + + if (fwTurn.s.stage == FW_INTO_MAIN) { + // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) + fwTurn.s.stage = (ABS(wrap_18000(c->legBearing - fwTurn.s.bOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; + } + if (fwTurn.s.stage == FW_INTO_IDLE && c->turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { + fwArcPlanFlyInto(c); + } else if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.stage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale + } +} + +// Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale +// out-bearing - unless the arc already flies toward that leg (the FLY_INTO pickup advances the +// WP mid-main-arc by design; retargeting would degrade the shaped arc to a bare capture) +static void fwArcRetargetOnLegChange(const fwArcCtx_t *c) +{ + if (ABS(wrap_18000(c->legBearing - fwTurn.arc.prevLegBearing)) > 500 + && ABS(wrap_18000(c->legBearing - fwTurn.arc.outBearing)) > 500) { + fwTurn.arc.flyByCappedLatch = false; + fwArcCaptureToLeg(c->legBearing); + if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.stage = FW_INTO_DONE; // staged S is stale: release the hand-back block + } + } + fwTurn.arc.prevLegBearing = c->legBearing; +} + +// S sequencer while an arc is engaged: bound the away arc, then pick up the second one +static void fwArcSequencerEngaged(const fwArcCtx_t *c) +{ + // Pick up the second arc at its tangency point. Along-track distance so a lateral residual + // cannot miss it; the heading gate blocks the trigger early in the first arc, where the + // pickup point still lies behind the exit course. + float outUx, outUy; + fwBearingUnit(fwTurn.arc.outBearing, &outUx, &outUy); + fwTurn.arc.pickupAlong = (fwTurn.s.ex - c->pos->x) * outUx + (fwTurn.s.ey - c->pos->y) * outUy; + + // The away arc blocks the hand-back, so a pickup that never triggers strands the aircraft with + // path tracking suppressed. Bound it by the time to fly half the away circle. + if (fwTurn.s.stage == FW_INTO_AWAY) { + fwTurn.s.awayMs += c->dtMs; + if (fwTurn.s.awayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * fwTurn.arc.r / MAX(c->v, NAV_FW_TURN_MIN_SPEED)) { + fwTurn.s.stage = FW_INTO_DONE; // release the block; the capture finishes on the leg + fwArcCaptureToLeg(c->legBearing); + } + } else { + fwTurn.s.awayMs = 0.0f; + } + + if (fwTurn.s.stage == FW_INTO_AWAY + && ABS(wrap_18000(fwTurn.arc.outBearing - c->cog)) < 4500 + && fwTurn.arc.pickupAlong <= fwRollInLeadCm(c->v, fwTurn.arc.tEaseMs)) { + const float blendFromCd = fwTurn.arc.bankCmd; // blend from the bank flown right now + // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the + // circle re-anchored along the leg line through the actual position: wind drift becomes + // an along-track shift instead of a parallel roll-out offset + const float r2 = getFwCoordinatedTurnRadius(); + const float legR2 = CENTIDEGREES_TO_RADIANS((float)fwTurn.s.bOut); + float u2x, u2y, b0x, b0y; + fwBearingUnit(fwTurn.s.bOut, &u2x, &u2y); + fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, + r2, legR2, (float)fwTurn.s.dir, &b0x, &b0y); + const float w2x = c->pos->x - b0x, w2y = c->pos->y - b0y; + const float w2u = w2x * u2x + w2y * u2y; + const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; + fwTurnPlan_t plan; + float cx, cy; + if (disc2 > 0.0f) { + const float t2 = w2u + fast_fsqrtf(disc2); + plan.r = r2; + cx = b0x + t2 * u2x; + cy = b0y + t2 * u2y; + plan.phiNomCd = fwBankForRadiusCd(c->v, r2); + plan.tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(plan.phiNomCd)); + } else { // drifted beyond the line: keep the planned circle and its bank + plan.r = fwTurn.s.r; + cx = fwTurn.s.o2x; + cy = fwTurn.s.o2y; + plan.phiNomCd = fwTurn.arc.phiNomCd; + plan.tEaseMs = fwTurn.arc.tEaseMs; + } + fwArcEngageRampIn(&plan, fwTurn.s.dir, cx, cy, fwTurn.s.bOut, blendFromCd); + fwTurn.s.stage = FW_INTO_MAIN; + if (c->turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { + /* Committed onto the outbound leg: mark the WP reached (as FLY_BY does at turn start). + * The corner cut never crosses the passage plane through the WP, so no geometric check + * can fire - the stale carrot would steer back toward the old leg after hand-back. */ + posControl.flags.wpTurnSmoothingActive = true; + } + } +} + +// Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - +// a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with +// the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. +static void fwArcLegLineCapture(const fwArcCtx_t *c) +{ + if (fwTurn.arc.toLegLine && navConfig()->fw.wp_tracking_accuracy) { + float legUx, legUy; + fwBearingUnit(c->legBearing, &legUx, &legUy); + const float offLeg = fwOffLegCm(c->pos->x, c->pos->y, + posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, legUx, legUy); + const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); + fwTurn.arc.outBearing = wrap_36000(c->legBearing - lrintf(SIGN(offLeg) * gammaCd)); + } +} + +// Roll-out lead: heading consumed by the shaped down-ramp plus the angle-P tail and servo delay +static float fwArcRollOutLeadCd(float v, float tEaseMs) +{ + const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); + const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); + const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] + const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; + return omegaCds * rollOutS + 0.5f * omegaCds * (tEaseMs / 1000.0f); +} + +static void fwArcStepRampIn(void) +{ + // Rate-consistent: a swing spanning 2*phi takes twice the standard ease time + const float rampSpanCd = fabsf((float)fwTurn.arc.dir * fwTurn.arc.phiNomCd - fwTurn.arc.rampStartCd); + const float rampDurMs = MAX(fwTurn.arc.tEaseMs * rampSpanCd / MAX(fwTurn.arc.phiNomCd, 1.0f), 0.5f * fwTurn.arc.tEaseMs); + const float p = (rampDurMs > 1.0f) ? constrainf(fwTurn.arc.rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; + fwTurn.arc.bankCmd = fwSmoothBlend(fwTurn.arc.rampStartCd, (float)fwTurn.arc.dir * fwTurn.arc.phiNomCd, p); // smoothstep up + if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle + fwTurn.arc.phase = ARC_STEADY; + fwTurn.arc.steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc + fwTurn.arc.steadyStartCd = fwTurn.arc.bankCmd; // law starts displaced: blend into it, don't step + } +} + +static void fwArcStepSteady(const fwArcCtx_t *c, int32_t hdgErrOut, float psiLeadCd, float maxStepCd) +{ + float eR; + int32_t eH; + const float steadyCd = fwArcLawCd(c->pos->x, c->pos->y, fwTurn.arc.cx, fwTurn.arc.cy, fwTurn.arc.r, fwTurn.arc.dir, c->cog, c->v, &eR, &eH); + fwTurn.arc.steadyMs += c->dtMs; + const float q = (fwTurn.arc.tEaseMs > 1.0f) ? constrainf(fwTurn.arc.steadyMs / fwTurn.arc.tEaseMs, 0.0f, 1.0f) : 1.0f; + // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre + // and eH can invert between two updates, which would otherwise go straight to the servos + const float blended = fwSmoothBlend(fwTurn.arc.steadyStartCd, steadyCd, q); + fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, blended, maxStepCd); + if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwTurn.arc.bankCmd) + || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it + fwTurn.arc.phase = ARC_CAPTURE; + } +} + +// Shaped roll-out onto the exit course; true = aligned and level, hand back to the PID +static bool fwArcStepCapture(int32_t hdgErrOut, float psiLeadCd, float maxStepCd) +{ + // No-overshoot envelope; slewed at the ramp rate in BOTH directions - the rise used to be + // unlimited, which stepped the servos at fresh capped engages and mid-capture mission advances + int32_t captureErr = hdgErrOut; + if (ABS(captureErr) > 17000) { + captureErr = fwTurn.arc.dir * ABS(captureErr); // ambiguous reversal: hold the engagement direction + } + const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); + const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -fwTurn.arc.phiNomCd, fwTurn.arc.phiNomCd); + fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, cmd, maxStepCd); + // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and + // path tracking a moment of control while we sit a full turn diameter off the leg. + return ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwTurn.arc.bankCmd) <= NAV_FW_ARC_EXIT_BANK_CD + && fwTurn.s.stage != FW_INTO_AWAY; // aligned and nearly level -> hand back +} + // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive // capture roll-out. Sets fwTurn.arc.active (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) @@ -850,19 +1211,22 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; if (turnMode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { - fwTurn.arc.engaged = false; - fwTurn.arc.prevLegBearing = -1; - fwTurn.s.stage = FW_INTO_IDLE; - fwArcDebugRelease(); + fwArcDisengageIdle(); return; } const int32_t legBearing = posControl.activeWaypoint.bearing; const int32_t cog = posControl.actualState.cog; - const int32_t hdgErrToLeg = wrap_18000(legBearing - cog); - const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; - const float v = posControl.actualState.velXY; - const float dtMs = US2S(deltaMicros) * 1000.0f; + const fwArcCtx_t ctx = { + .legBearing = legBearing, + .cog = cog, + .pos = &navGetCurrentActualPositionAndVelocity()->pos, + .v = posControl.actualState.velXY, + .dtMs = US2S(deltaMicros) * 1000.0f, + .turnMode = turnMode, + .hdgErrToLeg = wrap_18000(legBearing - cog), + .lastNavRollCmdCd = fwLastNavRollCmdCd, + }; if (!fwTurn.arc.engaged) { fwTurn.arc.toLegLine = false; @@ -870,313 +1234,49 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) * first tracked cycle is invisible - the loiter exit then never engages and the PID+FF fly * the whole turn unshaped. Unseeded + grossly off the leg course = a pending turn. */ const bool legChanged = (fwTurn.arc.prevLegBearing >= 0) - ? (ABS(wrap_18000(legBearing - fwTurn.arc.prevLegBearing)) > 500) - : (ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); - fwTurn.arc.prevLegBearing = legBearing; + ? (ABS(wrap_18000(ctx.legBearing - fwTurn.arc.prevLegBearing)) > 500) + : (ABS(ctx.hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); + fwTurn.arc.prevLegBearing = ctx.legBearing; if (legChanged) { fwTurn.s.stage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry const bool capped = fwTurn.arc.flyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwTurn.arc.flyByCappedLatch = false; // consume the latch on any leg change - if (turnMode == NAV_FW_WP_TURN_COORD_FLY_OVER) { - // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent - // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. - const float px = posControl.activeWaypoint.pos.x; - const float py = posControl.activeWaypoint.pos.y; - const int32_t brgToWp = fwRadToBearingCd(atan2_approx(py - pos->y, px - pos->x)); - const int32_t toWpErr = wrap_18000(brgToWp - cog); - if (ABS(toWpErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - fwTurnPlan_t plan; - fwPlanTurn(v, &plan); - const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; - const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); - // Pin ahead by the roll-in drift so the ramp ends ON the circle - const float leadDist = fwRollInLeadCm(v, plan.tEaseMs); - float leadX, leadY, cx, cy; - fwPolarOffset(pos->x, pos->y, leadDist, cogRad, &leadX, &leadY); - fwPerpOffset(leadX, leadY, plan.r, cogRad, dirTmp, &cx, &cy); - if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { - // Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the - // leg, gamma = half the turn for shallow corners), short straight for the reverse - // roll, then a standard corner-cut arc rolls out ON the line - const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - float ux, uy; - fwBearingUnit(legBearing, &ux, &uy); - const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(hdgErrToLeg), 2000.0f, 4500.0f)); - const float icptRad = legRad + (float)dirTmp * gammaRad; - const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); - const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); - float p1x, p1y, b0x, b0y, o2x, o2y; - fwPolarOffset(cx, cy, 2.0f * plan.r, nAng, &p1x, &p1y); // intercept line shifted R toward the counter side - fwPerpOffset(px, py, plan.r, legRad, -(float)dirTmp, &b0x, &b0y); - // gamma >= 20 deg keeps the lines well separated - if (fwLineIntersect(p1x, p1y, d1x, d1y, b0x, b0y, ux, uy, 0.17f, &o2x, &o2y)) { - const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; - const float legLen = calc_length_pythagorean_2D(px - pos->x, py - pos->y); - // roll-out tangency must lie ahead of us and leave straight leg to the WP - if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * plan.r) { - fwArcStageSecondArc(o2x, o2y, nAng, plan.r, legBearing, -dirTmp); - fwArcEngageRampIn(&plan, dirTmp, cx, cy, fwRadToBearingCd(icptRad), fwLastNavRollCmdCd); - fwTurn.s.stage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over - } - } - } - const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); - if (!fwTurn.arc.engaged && dCP > 1.05f * plan.r) { // next WP outside the circle: a tangent exists - const float alphaCP = atan2_approx(py - cy, px - cx); - const float phiT = acos_approx(constrainf(plan.r / dCP, 0.0f, 1.0f)); - for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP - const float th = alphaCP + (float)s * phiT; - float tx, ty, tdx, tdy; - fwPolarOffset(cx, cy, plan.r, th, &tx, &ty); - fwTangentDir(th, dirTmp, &tdx, &tdy); - if ((px - tx) * tdx + (py - ty) * tdy > 0.0f) { - fwArcEngageRampIn(&plan, dirTmp, cx, cy, - fwRadToBearingCd(atan2_approx(py - ty, px - tx)), fwLastNavRollCmdCd); - } - } - } - } - } - if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - fwTurnPlan_t plan; - fwPlanTurn(v, &plan); - const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / plan.r)); // v/R == g*tan(phi)/v - const float psiTmp = 0.5f * omegaNomCds * (plan.tEaseMs / 1000.0f); - if (capped || ABS(hdgErrToLeg) > NAV_FW_ARC_SHARP_TURN_CD) { - // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly - fwTurn.arc.engaged = true; - fwTurn.arc.bankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step - fwTurn.arc.rampMs = 0.0f; - fwTurn.arc.rampStartCd = 0.0f; - fwTurn.arc.r = plan.r; // capture ignores it, but the away timeout is sized from it - fwTurn.arc.dir = (hdgErrToLeg > 0) ? 1 : -1; - fwTurn.arc.phiNomCd = plan.phiNomCd; - fwTurn.arc.tEaseMs = plan.tEaseMs; - fwArcCaptureToLeg(legBearing); - } else if (2.0f * psiTmp < (float)ABS(hdgErrToLeg)) { // enough turn left for a steady arc between the ease ramps - const int8_t dirTmp = (hdgErrToLeg > 0) ? 1 : -1; - // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg - const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); - const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - float d1x, d1y, d2x, d2y, p1x, p1y, p2x, p2y, cx, cy; - fwBearingUnit(cog, &d1x, &d1y); - fwBearingUnit(legBearing, &d2x, &d2y); - fwPerpOffset(pos->x, pos->y, plan.r, cogRad, dirTmp, &p1x, &p1y); - fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - plan.r, legRad, dirTmp, &p2x, &p2y); - // legs not near-parallel (30..160 deg turn); degenerate -> tangent at the entry point - if (!fwLineIntersect(p1x, p1y, d1x, d1y, p2x, p2y, d2x, d2y, 0.087f, &cx, &cy)) { - cx = p1x; - cy = p1y; - } - fwArcEngageRampIn(&plan, dirTmp, cx, cy, legBearing, fwLastNavRollCmdCd); - } - } - } - if (!fwTurn.arc.engaged && turnMode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { - if (fwTurn.s.stage == FW_INTO_MAIN) { - // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) - fwTurn.s.stage = (ABS(wrap_18000(legBearing - fwTurn.s.bOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; - } - if (fwTurn.s.stage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { - const int32_t nta = posControl.activeWaypoint.nextTurnAngle; - if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { - fwTurnPlan_t plan; - fwPlanTurn(v, &plan); - const int8_t dirM = (nta > 0) ? 1 : -1; - const int32_t bOut = wrap_36000(legBearing + nta); - const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); - const float bInRad = CENTIDEGREES_TO_RADIANS((float)legBearing); - float ux, uy; - fwBearingUnit(legBearing, &ux, &uy); - // Main circle pinned at the WP, counter circle on the inbound leg; centers - // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room - const float Ls = 2.0f * v * (plan.tEaseMs / 1000.0f); - float o2x, o2y, ax, ay; - fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - plan.r, bOutRad, dirM, &o2x, &o2y); - fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - plan.r, bInRad, -(float)dirM, &ax, &ay); - const float wx = o2x - ax, wy = o2y - ay; - const float wu = wx * ux + wy * uy; - const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * plan.r * plan.r + Ls * Ls; - if (disc > 0.0f) { - const float s = wu - fast_fsqrtf(disc); // signed along-leg offset of the S start from the WP - const float triggerDist = -s + fwRollInLeadCm(v, plan.tEaseMs); - if (s < 0.0f && posControl.wpDistance < triggerDist) { - const float o1x = ax + s * ux; - const float o1y = ay + s * uy; - const float cAng = atan2_approx(o2y - o1y, o2x - o1x); - const float beta = atan2_approx(Ls, 2.0f * plan.r); - const float nAng = cAng + (float)dirM * beta; - fwArcStageSecondArc(o2x, o2y, nAng, plan.r, bOut, dirM); - // counter-arc first, rolling out onto the internal-tangent course - fwArcEngageRampIn(&plan, -dirM, o1x, o1y, - fwRadToBearingCd(cAng - (float)dirM * (M_PIf * 0.5f - beta)), fwLastNavRollCmdCd); - fwTurn.s.stage = FW_INTO_AWAY; - } - } - } - } else if (fwTurn.s.stage == FW_INTO_AWAY) { - fwTurn.s.stage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale - } + fwArcPlanNewLeg(&ctx, capped); } + fwArcSequencerIdle(&ctx); if (!fwTurn.arc.engaged) { DEBUG_SET(DEBUG_FW_TURN, 1, fwTurn.s.stage); return; } } else { - // Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale - // out-bearing - unless the arc already flies toward that leg (the FLY_INTO commit above advances - // the WP mid-main-arc by design; retargeting would degrade the shaped arc to a bare capture) - if (ABS(wrap_18000(legBearing - fwTurn.arc.prevLegBearing)) > 500 - && ABS(wrap_18000(legBearing - fwTurn.arc.outBearing)) > 500) { - fwTurn.arc.flyByCappedLatch = false; - fwArcCaptureToLeg(legBearing); - if (fwTurn.s.stage == FW_INTO_AWAY) { - fwTurn.s.stage = FW_INTO_DONE; // staged S is stale: release the hand-back block - } - } - fwTurn.arc.prevLegBearing = legBearing; - - // Pick up the second arc at its tangency point. Along-track distance so a lateral residual - // cannot miss it; the heading gate blocks the trigger early in the first arc, where the - // pickup point still lies behind the exit course. - float outUx, outUy; - fwBearingUnit(fwTurn.arc.outBearing, &outUx, &outUy); - fwTurn.arc.pickupAlong = (fwTurn.s.ex - pos->x) * outUx + (fwTurn.s.ey - pos->y) * outUy; - - // The away arc blocks the hand-back, so a pickup that never triggers strands the aircraft with - // path tracking suppressed. Bound it by the time to fly half the away circle. - if (fwTurn.s.stage == FW_INTO_AWAY) { - fwTurn.s.awayMs += dtMs; - if (fwTurn.s.awayMs > NAV_FW_ARC_AWAY_TIMEOUT_FACTOR * 1000.0f * M_PIf * fwTurn.arc.r / MAX(v, NAV_FW_TURN_MIN_SPEED)) { - fwTurn.s.stage = FW_INTO_DONE; // release the block; the capture finishes on the leg - fwArcCaptureToLeg(legBearing); - } - } else { - fwTurn.s.awayMs = 0.0f; - } - - if (fwTurn.s.stage == FW_INTO_AWAY - && ABS(wrap_18000(fwTurn.arc.outBearing - cog)) < 4500 - && fwTurn.arc.pickupAlong <= fwRollInLeadCm(v, fwTurn.arc.tEaseMs)) { - const float blendFromCd = fwTurn.arc.bankCmd; // blend from the bank flown right now - // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the - // circle re-anchored along the leg line through the actual position: wind drift becomes - // an along-track shift instead of a parallel roll-out offset - const float r2 = getFwCoordinatedTurnRadius(); - const float legR2 = CENTIDEGREES_TO_RADIANS((float)fwTurn.s.bOut); - float u2x, u2y, b0x, b0y; - fwBearingUnit(fwTurn.s.bOut, &u2x, &u2y); - fwPerpOffset(posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, - r2, legR2, (float)fwTurn.s.dir, &b0x, &b0y); - const float w2x = pos->x - b0x, w2y = pos->y - b0y; - const float w2u = w2x * u2x + w2y * u2y; - const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; - fwTurnPlan_t plan; - float cx, cy; - if (disc2 > 0.0f) { - const float t2 = w2u + fast_fsqrtf(disc2); - plan.r = r2; - cx = b0x + t2 * u2x; - cy = b0y + t2 * u2y; - plan.phiNomCd = fwBankForRadiusCd(v, r2); - plan.tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(plan.phiNomCd)); - } else { // drifted beyond the line: keep the planned circle and its bank - plan.r = fwTurn.s.r; - cx = fwTurn.s.o2x; - cy = fwTurn.s.o2y; - plan.phiNomCd = fwTurn.arc.phiNomCd; - plan.tEaseMs = fwTurn.arc.tEaseMs; - } - fwArcEngageRampIn(&plan, fwTurn.s.dir, cx, cy, fwTurn.s.bOut, blendFromCd); - fwTurn.s.stage = FW_INTO_MAIN; - if (turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { - /* Committed onto the outbound leg: mark the WP reached (as FLY_BY does at turn start). - * The corner cut never crosses the passage plane through the WP, so no geometric check - * can fire - the stale carrot would steer back toward the old leg after hand-back. */ - posControl.flags.wpTurnSmoothingActive = true; - } - } + fwArcRetargetOnLegChange(&ctx); + fwArcSequencerEngaged(&ctx); } - // Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - - // a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with - // the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. - if (fwTurn.arc.toLegLine && navConfig()->fw.wp_tracking_accuracy) { - float legUx, legUy; - fwBearingUnit(legBearing, &legUx, &legUy); - const float offLeg = fwOffLegCm(pos->x, pos->y, - posControl.activeWaypoint.pos.x, posControl.activeWaypoint.pos.y, legUx, legUy); - const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); - fwTurn.arc.outBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); - } + fwArcLegLineCapture(&ctx); - fwTurn.arc.rampMs += dtMs; + fwTurn.arc.rampMs += ctx.dtMs; fwTurn.arc.easeMs = fwTurn.arc.tEaseMs; // published for the handback fade - const int32_t hdgErrOut = wrap_18000(fwTurn.arc.outBearing - cog); - - // Roll-out lead: heading consumed by the shaped down-ramp plus the angle-P tail and servo delay - const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); - const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); - const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] - const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; - const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (fwTurn.arc.tEaseMs / 1000.0f); - const float maxStepCd = fwArcMaxStepCd(fwTurn.arc.phiNomCd, fwTurn.arc.tEaseMs, dtMs); + const int32_t hdgErrOut = wrap_18000(fwTurn.arc.outBearing - ctx.cog); + const float psiLeadCd = fwArcRollOutLeadCd(ctx.v, fwTurn.arc.tEaseMs); + const float maxStepCd = fwArcMaxStepCd(fwTurn.arc.phiNomCd, fwTurn.arc.tEaseMs, ctx.dtMs); switch (fwTurn.arc.phase) { - case ARC_RAMP_IN: { - // Rate-consistent: a swing spanning 2*phi takes twice the standard ease time - const float rampSpanCd = fabsf((float)fwTurn.arc.dir * fwTurn.arc.phiNomCd - fwTurn.arc.rampStartCd); - const float rampDurMs = MAX(fwTurn.arc.tEaseMs * rampSpanCd / MAX(fwTurn.arc.phiNomCd, 1.0f), 0.5f * fwTurn.arc.tEaseMs); - const float p = (rampDurMs > 1.0f) ? constrainf(fwTurn.arc.rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; - fwTurn.arc.bankCmd = fwSmoothBlend(fwTurn.arc.rampStartCd, (float)fwTurn.arc.dir * fwTurn.arc.phiNomCd, p); // smoothstep up - if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle - fwTurn.arc.phase = ARC_STEADY; - fwTurn.arc.steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc - fwTurn.arc.steadyStartCd = fwTurn.arc.bankCmd; // law starts displaced: blend into it, don't step - } + case ARC_RAMP_IN: + fwArcStepRampIn(); break; - } - case ARC_STEADY: { - float eR; - int32_t eH; - const float steadyCd = fwArcLawCd(pos->x, pos->y, fwTurn.arc.cx, fwTurn.arc.cy, fwTurn.arc.r, fwTurn.arc.dir, cog, v, &eR, &eH); - fwTurn.arc.steadyMs += dtMs; - const float q = (fwTurn.arc.tEaseMs > 1.0f) ? constrainf(fwTurn.arc.steadyMs / fwTurn.arc.tEaseMs, 0.0f, 1.0f) : 1.0f; - // Two-sided slew limit: on a tight arc the tangent bearing is ill-conditioned near the centre - // and eH can invert between two updates, which would otherwise go straight to the servos - const float blended = fwSmoothBlend(fwTurn.arc.steadyStartCd, steadyCd, q); - fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, blended, maxStepCd); - if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwTurn.arc.bankCmd) - || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it - fwTurn.arc.phase = ARC_CAPTURE; - } + case ARC_STEADY: + fwArcStepSteady(&ctx, hdgErrOut, psiLeadCd, maxStepCd); break; - } case ARC_CAPTURE: - default: { - // No-overshoot envelope; slewed at the ramp rate in BOTH directions - the rise used to be - // unlimited, which stepped the servos at fresh capped engages and mid-capture mission advances - int32_t captureErr = hdgErrOut; - if (ABS(captureErr) > 17000) { - captureErr = fwTurn.arc.dir * ABS(captureErr); // ambiguous reversal: hold the engagement direction - } - const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); - const float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -fwTurn.arc.phiNomCd, fwTurn.arc.phiNomCd); - fwTurn.arc.bankCmd = fwSlewToward(fwTurn.arc.bankCmd, cmd, maxStepCd); - // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and - // path tracking a moment of control while we sit a full turn diameter off the leg. - if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwTurn.arc.bankCmd) <= NAV_FW_ARC_EXIT_BANK_CD - && fwTurn.s.stage != FW_INTO_AWAY) { // aligned and nearly level -> hand back + default: + if (fwArcStepCapture(hdgErrOut, psiLeadCd, maxStepCd)) { fwTurn.arc.engaged = false; fwArcDebugRelease(); return; } break; } - } fwArcClampBankCmd(); From f85f5ff4a33b85890cd0a83128e462afa5dee64d Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:13:51 +0200 Subject: [PATCH 6/8] FW nav: make the turn coordinator's "WP reached" flag an event wpTurnSmoothingActive was cleared at the start of every position tick and re-asserted by FLY_BY while inside the lead distance, but FLY_INTO sets it exactly once at the S pickup. The consumer runs from the nav FSM at RX cadence, so the one-tick window was missed at random and the aircraft flew a full circle back to a waypoint it had already passed (151 deg corner, where no geometric check can fire). The flag now stays set until isWaypointReached consumes it; it is cleared on waypoint activation (as before) and on any nav state change, so a verdict cannot leak into RTH or landing states. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation.c | 4 +++- src/main/navigation/navigation_fixedwing.c | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 38f40895b47..36fbceae6b3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4085,6 +4085,7 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) if (posControl.navState != newState) { posControl.navState = newState; posControl.navPersistentId = navFSM[newState].persistentId; + posControl.flags.wpTurnSmoothingActive = false; // a turn's "WP reached" verdict is only valid in the state that produced it } return previousState; } @@ -4531,7 +4532,8 @@ bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBe uint16_t relativeBearingTargetAngle = 10000; if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { - // FLY_BY turn: the waypoint is reached when the anticipated corner-cut turn is initiated + // The turn coordinator declared the WP reached (FLY_BY at turn start, FLY_INTO at the S pickup). + // Set once, consumed here only - it must survive until this check runs, so nothing clears it per tick. posControl.flags.wpTurnSmoothingActive = false; return true; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 1e8fcb1bd92..8ab86f8eef1 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -1379,7 +1379,6 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t /* FLY_BY corner cut: start the turn R*tan(angle/2) before the WP so the arc joins the next leg * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); - posControl.flags.wpTurnSmoothingActive = false; // not fwEffectiveTurnMode(): the call costs 448 B of flash here at -O2 (F405), none at -Os const bool flyByLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_BY || posControl.navState == NAV_STATE_FW_LANDING_APPROACH; From d715e5cac6684dea61d1f9bc4ce4b4509f1e8b30 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:31:56 +0200 Subject: [PATCH 7/8] FW nav: trim the turn coordinator's comments to the why Multi-line blocks that narrated the code or its history are cut to one or two lines stating the constraint or measured fact behind it; comments that restated the statement below them are removed. Comments only, the binary is byte-identical. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation.c | 7 +- src/main/navigation/navigation_fixedwing.c | 118 ++++++++---------- .../navigation_fixedwing_turn_math.c | 1 - .../navigation_fixedwing_turn_math.h | 9 +- ...navigation_fixedwing_turn_math_unittest.cc | 9 +- 5 files changed, 57 insertions(+), 87 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 36fbceae6b3..fa521e7285f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4543,11 +4543,8 @@ bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBe return true; } - /* The bearing check above settles at (180 - turn angle) once the aircraft is established on the - * outbound leg, so beyond a ~80 deg turn it can only fire on the brief swing next to the WP. - * Miss that and the WP stays active behind the aircraft, which then turns back to it. Catch the - * pass geometrically: the WP is behind once the aircraft crosses the plane through it normal to - * the inbound leg. Cannot fire early - at WP activation the aircraft sits a leg length short. */ + /* Beyond a ~80 deg turn the bearing check above can only fire on the brief swing next to the WP, + * leaving the WP active behind the aircraft; the plane test cannot fire early (a leg length short). */ if (STATE(AIRPLANE) && FLIGHT_MODE(NAV_WP_MODE)) { const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float legRad = CENTIDEGREES_TO_RADIANS((float)*waypointBearing); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 8ab86f8eef1..bd639f0e458 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -79,13 +79,13 @@ #define NAV_FW_LOITER_RADIUS_DECAY 100.0f // [cm/s] max rate the held loiter radius eases back down (1 m/s) #define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance #define NAV_FW_ARC_MIN_TURN_ANGLE_CD 3000 // [centideg] only fly the coordinated arc for turns sharper than 30 deg -#define NAV_FW_ARC_RADIAL_GAIN 0.5f // [centideg bank / cm radial error] pull back onto the arc radius (TBD from flight) -#define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent heading error] align to the arc (TBD from flight) -#define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] proportional roll-out capture: bank -> 0 as cog reaches the out-leg (no overshoot) +#define NAV_FW_ARC_RADIAL_GAIN 0.5f // [centideg bank / cm radial error] hold the arc radius (TBD from flight) +#define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent error] align to the arc (TBD from flight) +#define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] roll-out capture, bank -> 0 at the out-leg #define NAV_FW_ARC_EXIT_HANDOFF_CD 500 // [centideg] hand back to the PID within this heading error of the out-leg -#define NAV_FW_ARC_EXIT_BANK_CD 1000.0f // [centideg] ... and below this arc bank command; the residual bank hands over into the tracker +#define NAV_FW_ARC_EXIT_BANK_CD 1000.0f // [centideg] ... and below this bank; the residual hands over into the tracker #define NAV_FW_ARC_AWAY_TIMEOUT_FACTOR 1.5f // half away-circle traverse times, before the away arc is abandoned -#define NAV_FW_ARC_SHARP_TURN_CD 15000 // [centideg] beyond this the tangent points explode toward the 180 deg reversal -> capture-only turn +#define NAV_FW_ARC_SHARP_TURN_CD 15000 // [centideg] beyond this the tangents explode toward the reversal -> capture only // FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) #define NAV_FW_GUARD_PHI_FLOOR_DEG 15.0f // minimum effective bank limit @@ -124,19 +124,17 @@ static float fwRollSmoothSeedCd = 0.0f; // baseline the smoother re-seeds to static float fwLastNavRollCmdCd = 0.0f; // last applied nav roll command [centideg] + timestamp, to tell a static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from a pilot handover at reset time -/* The turn predictor's entire static state, so its RAM cost is one sizeof per struct - * (see docs/development/ram-and-flash-optimization.md). */ +// The turn predictor's whole static state in one struct (docs/development/ram-and-flash-optimization.md) typedef struct { struct { float cx, cy, r; float phiNomCd; // coordinated nominal bank for this turn [centideg] float tEaseMs; // roll-in ease time float rampMs; // elapsed time in the ramp-in phase - float rampStartCd; // bank the ramp blends from (the live nav command on entry - a banked - // loiter exit must not level off first; the pickup blends mid-arc) + float rampStartCd; // bank the ramp blends from; a banked loiter exit must not level off first float steadyMs; // elapsed time in the steady phase float steadyStartCd; // bank the steady law blends from (the ramp's final command) - float bankCmd; // direct-radius arc bank command [centideg] (Approach B), applied to roll while active + float bankCmd; // arc bank command [centideg], applied to roll while active float easeMs; // ease time of the arc in progress, sizes the handback fade float pickupAlong; // along-track distance to the second-arc pickup [cm], for the log int32_t outBearing; @@ -144,8 +142,8 @@ typedef struct { uint8_t phase; int8_t dir; bool toLegLine; // fallback capture: converge onto the leg line itself, not just its course - bool active; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) - bool engaged; // arc latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption + bool active; // arc drives the roll this tick (-> bank headroom, cross-track suppressed) + bool engaged; // arc latch across loops; a controller reset must clear it or a stale arc resumes bool wasActive; // arc drove the roll last frame, to catch the release edge bool flyByCappedLatch; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc } arc; @@ -159,9 +157,9 @@ typedef struct { int8_t dir; } s; struct { - float cmdCd; // arc command at release; the PID/FF command is faded in from it so the - float ms; // seam is continuous regardless of nav_fw_control_smoothness (0 = no fade) - float durMs; + float cmdCd; // arc command at release; the PID/FF command fades in from it + float ms; // elapsed fade time + float durMs; // fade duration (0 = no fade in progress) } handback; struct { float cmdCd; // slew-limited turn feed-forward command [centideg] @@ -391,9 +389,8 @@ bool adjustFixedWingHeadingFromRCInput(void) static fpVector3_t virtualDesiredPosition; static pt1Filter_t fwCrossTrackErrorRateFilterState; -/* The controller reset's turn-predictor half. Deliberately partial: the S stage, the loiter - * ratchet/capture state and the guard's filters and latch survive a reset, and so do the arc - * geometry and command, which a still-established loiter circle keeps slewing from. */ +// Deliberately partial: S stage, loiter ratchet/capture, guard filters and the arc geometry survive +// a reset - a still-established loiter circle keeps slewing from them static void fwTurnStateReset(void) { fwTurn.arc.active = false; @@ -514,7 +511,7 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, active = false; // window elapsed -> back to 1:1 } else { const float p = elapsedMs / tConstMs; - out = fwSmoothBlend(rampStart, rollTargetCd, p); // smoothstep (S-curve) + out = fwSmoothBlend(rampStart, rollTargetCd, p); } } @@ -543,13 +540,13 @@ static float getFwPlanningBankDeg(void) return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); } -// Roll-command bank limit [deg]: an active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target +// Roll-command bank limit [deg]: an active arc may use the reserve up to the ceiling to hold its radius in wind static float getFwControlBankLimit(void) { return fwTurn.arc.active ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } -// Reduce the bank ceiling when a commanded climb stalls near the pitch/throttle limit while banked, so the turn widens and the climb recovers +// Reduce the bank ceiling when a commanded climb stalls while banked, so the turn widens and the climb recovers static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottleValue) { const float maxBank = getFwBankCeilingDeg(); @@ -644,9 +641,8 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr return 0.0f; // DIRECT = pure legacy PID } - // Turn-context gate: arm on a leg change, disarm once aligned. A later heading error on the same - // leg is a path-tracking correction (cross-track carrot), and the full coordinated bank on top of - // the PID turns every leg capture into an S-shaped swing. + // Arm on a leg change, disarm once aligned: a later heading error on the same leg is a tracking + // correction, and a full coordinated bank on top of the PID would make every capture an S-swing const int32_t ffLegBearing = posControl.activeWaypoint.bearing; if (fwTurn.ff.prevLegBearing < 0 || ABS(wrap_18000(ffLegBearing - fwTurn.ff.prevLegBearing)) > 500) { fwTurn.ff.armed = true; @@ -683,8 +679,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr return fwTurn.ff.cmdCd; } -// Stabilised loiter-radius floor [cm]: the raw requirement swings with wind (v^2) and would make the -// tracker thrash - ratchet up instantly, hold the peak one revolution, ease down at <= DECAY +// Stabilised loiter-radius floor [cm]: the raw v^2 requirement swings with wind and thrashes the tracker static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearingFromCenterRad, bool loiterActive, timeDelta_t deltaMicros) { const float required = getFwCoordinatedTurnRadius(); @@ -727,8 +722,7 @@ static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearing return out; } -// Roll-in/out ease time [ms] from roll rate, control_smoothness and the servo/inertia margin - -// single source of the turn's roll dynamics (the S-curve smoother is bypassed during the arc) +// Roll-in/out ease time [ms]: single source of the turn's roll dynamics (the S-curve smoother is bypassed during an arc) static float fwTurnEaseTimeMs(float phiNomDeg) { const float rollRateDps = currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f; @@ -860,9 +854,8 @@ typedef struct { float lastNavRollCmdCd; // bank an engage blends from; written only after this controller runs } fwArcCtx_t; -// Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the -// leg, gamma = half the turn for shallow corners), short straight for the reverse -// roll, then a standard corner-cut arc rolls out ON the line +// Tracking ON: exit the main arc on a bounded intercept course (<= 45 deg to the leg) so the +// following corner-cut arc rolls out ON the line, not parallel to it static void fwArcPlanFlyOverTrackingS(const fwArcCtx_t *c, const fwTurnPlan_t *plan, int8_t dirTmp, float cx, float cy, float px, float py) { @@ -1046,9 +1039,8 @@ static void fwArcSequencerIdle(const fwArcCtx_t *c) } } -// Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale -// out-bearing - unless the arc already flies toward that leg (the FLY_INTO pickup advances the -// WP mid-main-arc by design; retargeting would degrade the shaped arc to a bare capture) +// Mission advanced mid-arc: retarget onto the new leg, unless the arc already flies toward it - +// the FLY_INTO pickup advances the WP mid-arc by design and must keep its shaped arc static void fwArcRetargetOnLegChange(const fwArcCtx_t *c) { if (ABS(wrap_18000(c->legBearing - fwTurn.arc.prevLegBearing)) > 500 @@ -1065,9 +1057,8 @@ static void fwArcRetargetOnLegChange(const fwArcCtx_t *c) // S sequencer while an arc is engaged: bound the away arc, then pick up the second one static void fwArcSequencerEngaged(const fwArcCtx_t *c) { - // Pick up the second arc at its tangency point. Along-track distance so a lateral residual - // cannot miss it; the heading gate blocks the trigger early in the first arc, where the - // pickup point still lies behind the exit course. + // Along-track distance to the tangency point, so a lateral residual cannot miss the pickup; + // the heading gate blocks a trigger early in the first arc, where the point lies behind us float outUx, outUy; fwBearingUnit(fwTurn.arc.outBearing, &outUx, &outUy); fwTurn.arc.pickupAlong = (fwTurn.s.ex - c->pos->x) * outUx + (fwTurn.s.ey - c->pos->y) * outUy; @@ -1088,9 +1079,8 @@ static void fwArcSequencerEngaged(const fwArcCtx_t *c) && ABS(wrap_18000(fwTurn.arc.outBearing - c->cog)) < 4500 && fwTurn.arc.pickupAlong <= fwRollInLeadCm(c->v, fwTurn.arc.tEaseMs)) { const float blendFromCd = fwTurn.arc.bankCmd; // blend from the bank flown right now - // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the - // circle re-anchored along the leg line through the actual position: wind drift becomes - // an along-track shift instead of a parallel roll-out offset + // Radius from CURRENT groundspeed (the arming value may be unflyable downwind); re-anchoring + // the circle on the leg line turns wind drift into an along-track shift, not a parallel offset const float r2 = getFwCoordinatedTurnRadius(); const float legR2 = CENTIDEGREES_TO_RADIANS((float)fwTurn.s.bOut); float u2x, u2y, b0x, b0y; @@ -1119,17 +1109,15 @@ static void fwArcSequencerEngaged(const fwArcCtx_t *c) fwArcEngageRampIn(&plan, fwTurn.s.dir, cx, cy, fwTurn.s.bOut, blendFromCd); fwTurn.s.stage = FW_INTO_MAIN; if (c->turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { - /* Committed onto the outbound leg: mark the WP reached (as FLY_BY does at turn start). - * The corner cut never crosses the passage plane through the WP, so no geometric check - * can fire - the stale carrot would steer back toward the old leg after hand-back. */ + // Committed onto the outbound leg: the cut never crosses the WP passage plane, so no + // geometric check can fire and the stale carrot would steer back to the old leg posControl.flags.wpTurnSmoothingActive = true; } } } -// Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - -// a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with -// the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. +// Leg-line capture (path tracking on): steer onto the track itself, else a reversal fallback ends +// a turn diameter off the leg. Intercept angle tapers at 1 cd/cm, capped at the tracker's limit. static void fwArcLegLineCapture(const fwArcCtx_t *c) { if (fwTurn.arc.toLegLine && navConfig()->fw.wp_tracking_accuracy) { @@ -1158,11 +1146,12 @@ static void fwArcStepRampIn(void) const float rampSpanCd = fabsf((float)fwTurn.arc.dir * fwTurn.arc.phiNomCd - fwTurn.arc.rampStartCd); const float rampDurMs = MAX(fwTurn.arc.tEaseMs * rampSpanCd / MAX(fwTurn.arc.phiNomCd, 1.0f), 0.5f * fwTurn.arc.tEaseMs); const float p = (rampDurMs > 1.0f) ? constrainf(fwTurn.arc.rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; - fwTurn.arc.bankCmd = fwSmoothBlend(fwTurn.arc.rampStartCd, (float)fwTurn.arc.dir * fwTurn.arc.phiNomCd, p); // smoothstep up + fwTurn.arc.bankCmd = fwSmoothBlend(fwTurn.arc.rampStartCd, (float)fwTurn.arc.dir * fwTurn.arc.phiNomCd, p); if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle fwTurn.arc.phase = ARC_STEADY; - fwTurn.arc.steadyMs = 0.0f; // ramp-in does not steer onto the circle, so the arc - fwTurn.arc.steadyStartCd = fwTurn.arc.bankCmd; // law starts displaced: blend into it, don't step + // The ramp does not steer onto the circle, so the arc law starts displaced: blend, don't step + fwTurn.arc.steadyMs = 0.0f; + fwTurn.arc.steadyStartCd = fwTurn.arc.bankCmd; } } @@ -1186,8 +1175,8 @@ static void fwArcStepSteady(const fwArcCtx_t *c, int32_t hdgErrOut, float psiLea // Shaped roll-out onto the exit course; true = aligned and level, hand back to the PID static bool fwArcStepCapture(int32_t hdgErrOut, float psiLeadCd, float maxStepCd) { - // No-overshoot envelope; slewed at the ramp rate in BOTH directions - the rise used to be - // unlimited, which stepped the servos at fresh capped engages and mid-capture mission advances + // No-overshoot envelope, slewed at the ramp rate in BOTH directions: an unlimited rise steps + // the servos at fresh capped engages and at mid-capture mission advances int32_t captureErr = hdgErrOut; if (ABS(captureErr) > 17000) { captureErr = fwTurn.arc.dir * ABS(captureErr); // ambiguous reversal: hold the engagement direction @@ -1201,8 +1190,7 @@ static bool fwArcStepCapture(int32_t hdgErrOut, float psiLeadCd, float maxStepCd && fwTurn.s.stage != FW_INTO_AWAY; // aligned and nearly level -> hand back } -// Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive -// capture roll-out. Sets fwTurn.arc.active (drives the roll directly). +// Arc turn coordinator: ramp -> coordinated arc -> capture roll-out; sets fwTurn.arc.active (drives the roll) static void updateFwTurnArc(timeDelta_t deltaMicros) { fwTurn.arc.active = false; @@ -1230,9 +1218,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (!fwTurn.arc.engaged) { fwTurn.arc.toLegLine = false; - /* Unseeded (loiter/reset wipes the reference every cycle) means a WP advance landing on the - * first tracked cycle is invisible - the loiter exit then never engages and the PID+FF fly - * the whole turn unshaped. Unseeded + grossly off the leg course = a pending turn. */ + // Unseeded (a loiter or reset wipes the reference), so a WP advance on the first tracked + // cycle would be invisible: treat "unseeded and grossly off the leg course" as a pending turn const bool legChanged = (fwTurn.arc.prevLegBearing >= 0) ? (ABS(wrap_18000(ctx.legBearing - fwTurn.arc.prevLegBearing)) > 500) : (ABS(ctx.hdgErrToLeg) > NAV_FW_ARC_MIN_TURN_ANGLE_CD); @@ -1282,7 +1269,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (debugMode == DEBUG_FW_TURN) { debug[0] = lrintf(fwTurn.arc.r); // active arc radius [cm] - debug[1] = (fwTurn.arc.phase + 1) * 10 + fwTurn.s.stage; // coordinator state: (arc phase + 1)*10 + S stage + debug[1] = (fwTurn.arc.phase + 1) * 10 + fwTurn.s.stage; // coordinator state debug[2] = fwTurn.arc.outBearing; // exit course [centideg] debug[3] = hdgErrOut; // remaining heading to the exit course [centideg] debug[4] = lrintf(fwTurn.arc.bankCmd); // arc bank command [centideg] @@ -1294,8 +1281,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwTurn.arc.active = true; } -// Loiter circle controller: once established on the hold circle, the steady arc law replaces the -// carrot PID - live FF bank plus radial/tangent feedback hold the stabilised radius exactly +// Loiter circle controller: once established, the steady arc law replaces the carrot PID to hold the radius exactly static void updateFwLoiterArc(timeDelta_t deltaMicros) { if (!needToCalculateCircularLoiter || fwTurn.arc.engaged) { @@ -1352,8 +1338,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t uint32_t navLoiterRadius = getLoiterRadius(navigationGetLoiterRadius()); - /* Loiter-radius floor with per-revolution peak hold (see getFwStableLoiterRadius): keep the circle - * stable for the fixed-radius loiter tracker instead of chasing the wind-varying instantaneous value. */ + // The fixed-radius loiter tracker needs a stable circle, not the wind-varying instantaneous value const bool inLoiter = (navGetCurrentStateFlags() & NAV_CTL_HOLD); const float bearingFromCenter = atan2_approx(-posErrorY, -posErrorX); navLoiterRadius = getFwStableLoiterRadius(navLoiterRadius, bearingFromCenter, inLoiter, deltaMicros); @@ -1388,7 +1373,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t // Roll-in lead: the ramp is back-loaded and the ground track lags the bank (k calibrated in flight) const float easeLeadDistance = posControl.actualState.velXY * (fwTurnEaseTimeMs(getFwPlanningBankDeg()) / 1000.0f) * 1.5f; float turnStartDistance = easeLeadDistance + turnRadius * halfAngleTan; - // Cap how early the turn may begin (nav_fw_wp_turn_max_lead_time): never more than N ms of flight before the WP. + // nav_fw_wp_turn_max_lead_time: never begin the turn more than N ms of flight before the WP const float maxLeadDistance = posControl.actualState.velXY * (float)navConfig()->fw.wp_turn_max_lead_time * 0.001f; const bool turnCapped = turnStartDistance > maxLeadDistance; turnStartDistance = MIN(turnStartDistance, maxLeadDistance); @@ -1411,7 +1396,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY); } - // Arc turn coordinator: manages the turn state and commands the roll bank directly + // These command the roll bank directly, not the virtual target computed below updateFwTurnArc(deltaMicros); updateFwLoiterArc(deltaMicros); @@ -1533,9 +1518,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } else { - /* Keep state synced to the current error while not steering, and seed the convergence - * estimate from the geometric closing speed: re-engaging with a zero rate reads as - * "not converging" and commands the full correction in one step (arc hand-back kick). */ + // Seed the convergence estimate from the geometric closing speed: re-engaging with a + // zero rate reads as "not converging" and commands the full correction in one step previousCrossTrackError = navCrossTrackError; previousCrossTrackErrorUpdateTime = currentTimeUs; const fpVector3_t *trackPos = &navGetCurrentActualPositionAndVelocity()->pos; @@ -1612,7 +1596,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta fwTurn.handback.durMs = fwTurn.arc.easeMs; fwTurn.arc.wasActive = false; } - // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. + // The FF supplies the turn's bank so the PID only trims rollAdjustment += getFwTurnFeedForward(navHeadingError, deltaMicros); rollAdjustment = applyFwArcHandbackFade(rollAdjustment, deltaMicros); rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); diff --git a/src/main/navigation/navigation_fixedwing_turn_math.c b/src/main/navigation/navigation_fixedwing_turn_math.c index 77bb49663a2..cb2b9d9fff4 100644 --- a/src/main/navigation/navigation_fixedwing_turn_math.c +++ b/src/main/navigation/navigation_fixedwing_turn_math.c @@ -83,7 +83,6 @@ bool fwLineIntersect(float p1x, float p1y, float d1x, float d1y, return true; } -// Smoothstep interpolation from -> to over the normalised progress p. // NOINLINE (F7/H7 only): measured smaller than letting LTO re-inline it at every call site NOINLINE float fwSmoothBlend(float from, float to, float p) { diff --git a/src/main/navigation/navigation_fixedwing_turn_math.h b/src/main/navigation/navigation_fixedwing_turn_math.h index 807624a90b6..4656cdaef61 100644 --- a/src/main/navigation/navigation_fixedwing_turn_math.h +++ b/src/main/navigation/navigation_fixedwing_turn_math.h @@ -20,13 +20,8 @@ #include #include -/* Shared turn-geometry primitives of the fixed-wing turn coordinator. - * - * The expressions below are frozen: navigation_fixedwing_turn_math_unittest.cc - * checks them bit-exactly against the inline code they replaced, so operand - * order, the (x / RAD) * 100 unit idiom and the lrintf placement are part of - * the contract, not style. Changing any of them changes flight behaviour. - */ +/* Shared turn-geometry primitives of the fixed-wing turn coordinator. The expressions are frozen - + * the unit test checks them bit-exactly, so operand order and unit idioms are contract, not style. */ float fwBankForRadiusCd(float v, float radiusCm); int32_t fwRadToBearingCd(float rad); diff --git a/src/test/unit/navigation_fixedwing_turn_math_unittest.cc b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc index 47d85b15a1b..b01206fde7d 100644 --- a/src/test/unit/navigation_fixedwing_turn_math_unittest.cc +++ b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc @@ -15,13 +15,8 @@ * along with INAV. If not, see . */ -/* Equivalence tests for the turn-geometry helpers extracted from - * navigation_fixedwing.c. Every ref_* function below is a verbatim copy of the - * inline expression the helper replaced (the comment gives the call site at - * d1a87ed6c). The comparison is bit-exact, not approximate: the turn coordinator - * latches on centidegree thresholds, so a 1 ulp drift can change a phase - * transition. A failure here means the refactor changed flight behaviour. - */ +/* Every ref_* function is a verbatim copy of the inline expression its helper replaced (the comment + * gives the call site at d1a87ed6c); bit-exact, because the coordinator latches on centidegree steps. */ #include #include From a5bd0b9c053b6b0ec0275a459ab0ad4a871e8c0d Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 21 Sep 2026 22:06:20 +0200 Subject: [PATCH 8/8] FW nav: keep the NaN fallback of the line-intersection guard The extracted helper rejected with `fabsf(cross) <= min`, which lets a NaN cross product through and would engage an arc on NaN coordinates; the original sites tested `> min` and fell back. Restored as the positive test, with a NaN case in the unit test. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_fixedwing_turn_math.c | 5 +++-- src/test/unit/navigation_fixedwing_turn_math_unittest.cc | 4 ++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing_turn_math.c b/src/main/navigation/navigation_fixedwing_turn_math.c index cb2b9d9fff4..8b54fa6e18c 100644 --- a/src/main/navigation/navigation_fixedwing_turn_math.c +++ b/src/main/navigation/navigation_fixedwing_turn_math.c @@ -67,13 +67,14 @@ void fwPerpOffset(float px, float py, float r, float headingRad, float dirF, flo fwPolarOffset(px, py, r, headingRad + dirF * (M_PIf * 0.5f), ox, oy); } -// Intersection of the lines p1 + t*d1 and p2 + s*d2; false (outputs untouched) when near-parallel +// Intersection of the lines p1 + t*d1 and p2 + s*d2; false (outputs untouched) when near-parallel. +// Written as the positive test so a NaN cross product also lands on the fallback, as at the call sites bool fwLineIntersect(float p1x, float p1y, float d1x, float d1y, float p2x, float p2y, float d2x, float d2y, float minAbsCross, float *ox, float *oy) { const float cross = d1x * d2y - d1y * d2x; - if (fabsf(cross) <= minAbsCross) { + if (!(fabsf(cross) > minAbsCross)) { return false; } diff --git a/src/test/unit/navigation_fixedwing_turn_math_unittest.cc b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc index b01206fde7d..1082b0c1ada 100644 --- a/src/test/unit/navigation_fixedwing_turn_math_unittest.cc +++ b/src/test/unit/navigation_fixedwing_turn_math_unittest.cc @@ -299,6 +299,10 @@ TEST(NavFwTurnMathUnittest, LineIntersectDegenerate) EXPECT_FALSE(fwLineIntersect(p1x, p1y, 1.0f, 0.0f, p2x, p2y, -1.0f, 0.0f, 0.087f, &ox, &oy)); EXPECT_BITS_EQ(ox, 1.0f); // outputs untouched on the degenerate path EXPECT_BITS_EQ(oy, 2.0f); + // NaN geometry must take the fallback, like the original `> threshold` guard + EXPECT_FALSE(fwLineIntersect(p1x, p1y, NAN, 0.0f, p2x, p2y, 0.0f, 1.0f, 0.087f, &ox, &oy)); + EXPECT_BITS_EQ(ox, 1.0f); + EXPECT_BITS_EQ(oy, 2.0f); // cross exactly on / just below / just above each threshold for (float thr : { 0.087f, 0.17f }) {