Draft — Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804)

docs/Fixed Wing Landing.md

@@ -20,6 +20,8 @@ This enables up to 4 different approach directions, based on the landing site an
 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held
 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`.
 
+All turns between the approach legs are flown as coordinated FLY_BY corner cuts: the turn start is anticipated from the aircraft's actual turn radius (speed and `nav_fw_bank_angle`) so the plane rolls out already aligned on the next leg. This applies during the landing approach regardless of the configured `nav_fw_wp_turn_mode`.
+
 To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`. 
 
 > [!WARNING]
@@ -90,8 +92,8 @@ If WP-Tracking is not used, the Plane will head straight to the landiung locatio
 * `nav_fw_pitch2thr`: The navigation throttle modifier has to be tuned well to allow stable navigation during climbs and descents to prevent a stall. Make sure your plane maintains Ground or Airspeed, when climbing in any navigation mode. 
 The Craft should not get slower and not speed ub significantly during a navigation climb, if P2T is tuned properly. See `Fixed Wing Pitch To Throttle Tuning.md` for a full tuning procedure.
 
-* `nav_wp_radius`: This parameter might be too high if you have set up your craft with INAV 6 or INAV 7. With a too high value, the turning points for the Crosswind-Leg and Final Approach are hit too early and make it difficult for the plane to align to the runway or cut short the approach. 
-Make sure this parameter is not set greater than 1000 (cm). The better your craft and navigation system is tuned, the lower this value can be. We recommend to start with 1000 for flying wings and 800 for a Plane with Tail.
+* `nav_wp_radius`: The turning points for the Crosswind-Leg and Final Approach are calculated automatically from the aircraft's coordinated turn radius; `nav_wp_radius` does not shape the approach corners. 
+Make sure this parameter is not set greater than 1000 (cm) so approach waypoints are not detected as reached too early.
 
 * Test your Navigation-Tuning: A better Navigation-Tune will reward you with smoother and more reliable landings. To test your nav systems limit, we recommend to create a waypoint missions with many 90° turn angles with shorter and shorter tracks. 
 With this Method, you can find out how well your plane can follow a navigation path and how long it takes to align to a waypoint track. A well tuned plane should be able to pull of a WP Mission that looks like this, where the distance between WP6 and WP7 si recommended to be the minimum approach length: 

docs/Settings.md

@@ -795,6 +795,7 @@ Defines debug values exposed in debug variables (developer / debugging setting)
 | LULU |  |
 | SBUS2 |  |
 | OSD_REFRESH |  |
+| FW_TURN |  |
 
 ---
 
@@ -3507,6 +3508,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
 
 ---
 
+### nav_cruise_lock_on_level
+
+Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
 ### nav_cruise_yaw_rate
 
 Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]
@@ -3620,7 +3631,7 @@ P gain of auto speed PID controller.
 
 ### nav_fw_bank_angle
 
-Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
+Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -3640,7 +3651,7 @@ Max pitch angle when climbing in GPS assisted modes, is also restrained by globa
 
 ### nav_fw_control_smoothness
 
-How smoothly the autopilot controls the airplane to correct the navigation error
+How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -4138,6 +4149,16 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within
 
 ---
 
+### nav_fw_turn_ff_gain
+
+Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
 ### nav_fw_wp_tracking_accuracy
 
 Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy.
@@ -4158,15 +4179,36 @@ Sets the maximum allowed alignment convergence angle to the waypoint course line
 
 ---
 
-### nav_fw_wp_turn_smoothing
+### nav_fw_wp_turn_control_ease
 
-Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner).
+Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 500 |
+
+---
+
+### nav_fw_wp_turn_max_lead_time
+
+COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 6000 | 3000 | 12000 |
+
+---
+
+### nav_fw_wp_turn_mode
+
+How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries).
 
 | Allowed Values |  |
 | --- | --- |
-| OFF | Default |
-| ON |  |
-| ON-CUT |  |
+| DIRECT |  |
+| COORD_FLYBY | Default |
+| COORD_FLYOVER |  |
+| COORD_FLYINTO |  |
 
 ---
 

docs/development/fw-turn-debugging.md

@@ -0,0 +1,59 @@
+# Fixed-wing coordinated turn debugging (`DEBUG_FW_TURN`)
+
+`set debug_mode = FW_TURN` exposes the fixed-wing coordinated WP turn system: the arc turn
+coordinator, the S sequencer (COORD_FLYOVER with path tracking / COORD_FLYINTO), the loiter
+radius stabiliser, the turn/loiter feed-forward and the energy bank guard. Values are available
+via the CLI `debug` command, the `OSD_DEBUG` element, MSP `DEBUGMSG`/`DEBUG` telemetry and
+blackbox logging.
+
+## Channels
+
+| debug[] | Value | Unit | Written by |
+|---|---|---|---|
+| 0 | Active turn/loiter radius | cm | Loiter stabiliser every cycle; overridden by the COORD_FLYBY planning radius while a corner approach is active, and by the engaged arc's radius while the coordinator flies the turn (latest writer wins) |
+| 1 | Coordinator state (see below) | – | Arc coordinator |
+| 2 | Exit course of the active arc | centideg | Arc coordinator, only while engaged |
+| 3 | Remaining heading to the exit course | centideg | Arc coordinator, only while engaged |
+| 4 | Arc bank command | centideg | Arc coordinator, only while engaged; clamped to the effective bank ceiling |
+| 5 | WP-turn roll feed-forward | centideg | Feed-forward (0 when disabled, loitering, or inside the heading deadband) |
+| 6 | Energy-guard bank ceiling | deg | Energy bank guard (sits at `max_angle_inclination_rll` unless the guard is reducing it) |
+| 7 | Roll ease time | ms | Arc coordinator, only while engaged (sizes the entry/exit ramps and the turn-start lead) |
+
+Channels 2, 3, 4 and 7 hold their last value after the arc disengages; check channel 1 to know
+whether the coordinator is active.
+
+## Channel 1: coordinator state
+
+While the arc coordinator is engaged the value is `(arc phase + 1) * 10 + S stage`; while idle
+it is the S stage alone. Two loiter states use their own values: `4` = holding, approach on the
+stock carrot guidance; `40` = loiter circle controller engaged (steady arc law holds the
+radius).
+
+Arc phase (tens digit):
+
+| Digit | Phase | Meaning |
+|---|---|---|
+| 1 | RAMP_IN | Smoothstep bank ramp onto the pre-placed circle |
+| 2 | STEADY | Coordinated arc: live feed-forward bank + radius/tangent feedback |
+| 3 | CAPTURE | Predictive roll-out onto the exit course |
+
+S stage (ones digit; 0 outside the S modes):
+
+| Digit | Stage | Meaning |
+|---|---|---|
+| 0 | – | Plain single-arc turn (COORD_FLYBY, COORD_FLYOVER tangent exit) |
+| 1 | AWAY | First arc of the S; the second arc is staged |
+| 2 | MAIN | Second arc of the S (COORD_FLYINTO: the aligned WP crossing; COORD_FLYOVER + tracking: the corner-cut onto the leg) |
+| 3 | DONE | S completed for this leg, waiting for the next leg switch |
+
+Examples: `20` = flying a plain coordinated turn; `21` = steady on the first S arc; `32` =
+rolling out of the S's second arc; `3` (idle) = S finished, coordinator handed back.
+
+## Reading a turn
+
+A healthy COORD_FLYBY corner shows: channel 1 stepping `10 → 20 → 30 → 0`, channel 4 ramping to
+the nominal bank, holding, then collapsing as channel 3 converges to 0 without overshoot. The
+hand-back happens aligned (|ch3| small) and nearly level (|ch4| < 10% of nominal). Channel 4
+pegged at channel 6 × 100 indicates the command is saturated at the flyable ceiling — expected
+briefly downwind, a problem if sustained. Channel 6 dropping below `max_angle_inclination_rll`
+means the energy guard is trading bank for climb capability.

src/main/build/debug.h

@@ -80,6 +80,7 @@ typedef enum {
     DEBUG_LULU,
     DEBUG_SBUS2,
     DEBUG_OSD_REFRESH,
+    DEBUG_FW_TURN,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/cms/cms_menu_navigation.c

@@ -202,7 +202,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
     OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
 #endif
     OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART),
-    OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING),
+    OSD_SETTING_ENTRY("WP TURN MODE", SETTING_NAV_FW_WP_TURN_MODE),
     OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY),
     OSD_BACK_AND_END_ENTRY,
  };

src/main/fc/cli.c

@@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = {
     "GPS",
     "LULU",
     "SBUS2",
-    "OSD_REFRESH"
+    "OSD_REFRESH",
+    "FW_TURN"
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status

src/main/fc/settings.yaml

@@ -84,7 +84,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
       "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
-      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"]
+      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "FW_TURN"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
@@ -180,9 +180,9 @@ tables:
   - name: dynamic_gyro_notch_mode
     values: ["2D", "3D"]
     enum: dynamicGyroNotchMode_e
-  - name: nav_fw_wp_turn_smoothing
-    values: ["OFF", "ON", "ON-CUT"]
-    enum: wpFwTurnSmoothing_e
+  - name: nav_fw_wp_turn_mode
+    values: ["DIRECT", "COORD_FLYBY", "COORD_FLYOVER", "COORD_FLYINTO"]
+    enum: navFwWpTurnMode_e
   - name: gps_auto_baud_max
     values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600']
     enum: gpsBaudRate_e
@@ -2670,11 +2670,29 @@ groups:
         field: fw.wp_tracking_max_angle
         min: 30
         max: 80
-      - name: nav_fw_wp_turn_smoothing
-        description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner)."
-        default_value: "OFF"
-        field: fw.wp_turn_smoothing
-        table: nav_fw_wp_turn_smoothing
+      - name: nav_fw_wp_turn_mode
+        description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries)."
+        default_value: "COORD_FLYBY"
+        field: fw.wp_turn_mode
+        table: nav_fw_wp_turn_mode
+      - name: nav_fw_turn_ff_gain
+        description: "Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven."
+        default_value: 100
+        field: fw.turn_ff_gain
+        min: 0
+        max: 200
+      - name: nav_fw_wp_turn_max_lead_time
+        description: "COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint."
+        default_value: 6000
+        field: fw.wp_turn_max_lead_time
+        min: 3000
+        max: 12000
+      - name: nav_fw_wp_turn_control_ease
+        description: "Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes."
+        default_value: 100
+        field: fw.wp_turn_control_ease
+        min: 0
+        max: 500
       - name: nav_auto_speed
         description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
         default_value: 500
@@ -2869,6 +2887,11 @@ groups:
         field: general.cruise_yaw_rate
         min: 0
         max: 120
+      - name: nav_cruise_lock_on_level
+        description: "Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour)."
+        default_value: ON
+        field: general.cruise_lock_on_level
+        type: bool
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
         default_value: 35
@@ -2967,7 +2990,7 @@ groups:
         field: mc.slowDownForTurning
         type: bool
       - name: nav_fw_bank_angle
-        description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
+        description: "Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll"
         default_value: 35
         field: fw.max_bank_angle
         min: 5
@@ -3056,7 +3079,7 @@ groups:
         min: 1
         max: 10
       - name: nav_fw_control_smoothness
-        description: "How smoothly the autopilot controls the airplane to correct the navigation error"
+        description: "How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing."
         default_value: 0
         field: fw.control_smoothness
         min: 0