Fixed wing: flash/RAM pass on the coordinated turn coordinator (behaviour-identical refactor)

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

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;
     }
@@ -4541,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);

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#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.
+// 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)) {
+        return false;
+    }
+
+    const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross;
+    *ox = p1x + tt * d1x;
+    *oy = p1y + tt * d1y;
+    return true;
+}
+
+// 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);
+}

src/main/navigation/navigation_fixedwing_turn_math.h

@@ -0,0 +1,39 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* 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);
+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);

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)