Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804) - #11812
Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804) #11812b14ckyy wants to merge 33 commits into
Conversation
…y-state lag) nav_fw_control_smoothness applied a PT1 low-pass to the FW nav roll command. That trades smoothness for a permanent, uncompensated lag between what the navigation controller commands and what is executed: every course correction is delayed, also during steady tracking where no smoothing is needed, and the lag grows with the smoothness setting. Replace the roll-axis PT1 with a triggered S-curve easing: - Fires only on an abrupt commanded-bank step (setpoint-rate change above 20% of the configured roll rate between nav loops), e.g. a new course at a waypoint or a nav-mode entry (RTH engage, WP start). - Eases from the pre-step output to the live target with a smoothstep over a control_smoothness-derived window (n x 100 ms, 0 = off, capped at 1000 ms), then passes the command 1:1 again. - The window timer does not reset on further steps mid-ramp, so the smoother can never get stuck damping steady tracking. - On position-controller reset the smoother re-seeds from the last applied nav roll command when nav was commanding until just now (nav-mode to nav-mode transition, e.g. RTH -> CRUISE: the level-off is eased), and from the neutral baseline after a pilot-flown phase (stick release: a roll-out in progress is not re-commanded). Stale state can never fire a spurious ramp. Same knob, same range and same intent (soft control feel, structural protection on large airframes); the pitch/pitch-to-throttle PT1 smoothing is deliberately unchanged. No settings or PG layout changes. HITL-tested on real hardware (window rescaled to n x 100 ms from flight observation; re-seed behavior derived from RTH engage, cruise stick release and RTH->CRUISE fallback tests).
In COURSE_HOLD/CRUISE the course is locked the moment the mode engages or the pilot releases the stick (roll-stick path: last course stored in ADJUSTING; yaw path: on release with a one-iteration gyro lead; mode entry: in INITIALIZE). If the aircraft is still banked at that moment - stick released mid-turn, or the mode switched out of e.g. an RTH turn - it keeps turning through the level-off, overshoots the locked course and flies a reverse correction turn. A longstanding annoyance, made more visible by softer roll-out (control smoothing). Delay the course lock until the roll-out is actually complete: while the bank is above 10 deg the course keeps following the actual COG (roll-stick path stays in ADJUSTING; yaw release and banked mode entry share one lock-pending flag), then locks with the gyro-lead compensation. The course now locks where the aircraft has effectively stopped turning - no overshoot, no reverse correction - and the controller reset/re-engage happens near wings-level, so the smoothing re-seed cannot cause a roll jerk. Fixed-wing only; multicopter course hold is unaffected.
…asing cap to 900ms The course-lock applied 'cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW))', mixing a rate (deg/s) into an angle - effectively a fixed one-second yaw lead. With the new bank gate the turn has essentially stopped at lock time, so lock directly to the current COG. NAV_FW_SMOOTH_TCONST_MAX_MS claimed a 1000ms cap that was unreachable with control_smoothness max 9 (n x 100ms = 900ms); set the cap and comments to 900ms to match the setting range and documentation.
…fault ON) On maintainer feedback the level-off course lock in course hold is a behavior change, so make it optional: ON locks the course only once rolled out below 10 deg bank (new behavior), OFF locks on stick center / mode entry as before. Bumps PG_NAV_CONFIG to 9. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
|
ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing |
PR Summary by QodoFixed-wing: predictive coordinated waypoint turns + loiter arc control
AI Description
Diagram
High-Level Assessment
Files changed (11)
|
Code Review by Qodo
1.
|
![qodo-free-for-open-source-projects[bot]](https://avatars.githubusercontent.com/in/1068000?s=60&v=4){kind=small}
|
RAM / Flash usage vs. base branch — commit
See RAM/flash optimization guide for techniques to reduce usage. |
|
Test firmware build ready — commit Download firmware for PR #11812 246 targets built. Find your board's
|
A badly tuned model in strong wind can stay above the 10 deg bank gate indefinitely, leaving the course lock pending. Once the roll/yaw sticks are centred the lock now engages after 2500 ms regardless of bank. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
473588f to
41b058d
Compare
A yaw-stick turn holds the course setpoint ahead of the actual COG, winding up the nav PID integrator to the bank that sustains the turn. Once the stick centres the course follows COG, so the error is zero and neither the error term nor back-calculation (only active while saturated) can unwind it: the residual bank kept the turn going and blocked the roll-out the course lock waits for. Reset the position controller once when the adjustment ends, as the pre-existing code did before the lock-on-level gate was introduced. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Replace static-radius nav_fw_wp_turn_smoothing with nav_fw_wp_turn_mode. FLY_BY anticipates the turn from the real coordinated-turn radius R = V^2/(g*tan(bank)) at d = R*tan(angle/2), so the turn starts at the correct distance at any speed; FLY_OVER flies over the WP then turns. Landing approach always uses FLY_BY. PG_NAV_CONFIG 7->8.
…curve roll-in Builds on the FLY_BY/FLY_OVER turn predictor: - Energy/altitude bank guard (always-on): reduce the effective nav bank limit when a commanded climb cannot be sustained near the pitch/throttle limit while banked, from target-vs-actual vertical speed (filtered + Schmitt deadband + bank-entry baseline). Widens the turn/loiter so the climb recovers. - Loiter-radius floor: never demand a circle tighter than the effective bank allows. - Coordinated-turn feed-forward (nav_fw_turn_ff_gain, dev/experimental, default 100): command the geometric bank for the active turn/loiter radius so the PID only trims. - Roll-in S-curve replacing the control_smoothness PT1 (roll axis only): step-triggered, control_smoothness*50ms time constant, smoothstep then 1:1. Pitch PT1 retained. - DEBUG_FW_TURN channel for tuning. PG_NAV_CONFIG 8->9. (Includes minor comment trims to the B1 turn-predictor code per AGENT.md.)
…iter-only headroom Refinements on the B2 feed-forward/guard work (navigation_fixedwing.c): - Loiter-radius floor -> stabilised per-revolution peak hold with 1 m/s gradual decay (getFwStableLoiterRadius): ratchet up at once, hold the peak over a full revolution (orbital azimuth net 360deg), ease down at <=1 m/s. Stops the commanded circle thrashing with wind-driven ground-speed swings. - Bank-limit semantics: nav_fw_bank_angle is the planning TARGET; control output may use reserve up to the hard ceiling max_angle_inclination_rll to hold the radius (getFwBankCeilingDeg / getFwEffectiveBankLimit / getFwPlanningBankDeg). Energy guard reduces the ceiling and snaps straight to the target on trigger. - Headroom is loiter-only (getFwControlBankLimit): WP turns/cruise clamp to the planning target so coordinated turns fly a clean arc, not the hard ceiling. - Roll-in S-curve time constant -> control_smoothness*100ms (cap 1000ms).
Replace the heading-PID corner turn with an explicit coordinated arc on real WP-to-WP turns (>30 deg). New nav_fw_wp_turn_coordination = COORDINATED (default) / DIRECT (legacy fallback). The turn is a variable-radius spline driven directly on the roll axis: - RAMP_IN: smoothstep bank 0->phi_nom (no servo slam; control_smoothness folded into the ramp time and bypassed during the arc). - STEADY: direct radius control (nominal + radial pull-back + tangent alignment) against an inscribed circle placed tangent to BOTH legs, so the exit lands on the out-leg instead of offset. - CAPTURE: closed-loop roll-out, bank proportional to the heading still to go -> levels exactly on the out-leg, cannot overshoot the heading. Roll-aware easing: ease time = 1.5*phi/roll_rate + control_smoothness + the new nav_fw_wp_turn_control_ease (servo/inertia margin); the FLY_BY turn-start lead is sized from it so the longer eased path still starts in time. PG_NAV_CONFIG 10 -> 11. Settings: nav_fw_wp_turn_coordination, nav_fw_wp_turn_handback_angle, nav_fw_wp_turn_max_lead_time, nav_fw_wp_turn_control_ease (all dev/experimental).
Two state-handling fixes in the arc turn coordinator (found in code review): - The coordinator's engage latch and leg-bearing memory were function-local statics that survived resetFixedWingPositionController(). Interrupting nav mid-arc (switch to ANGLE/ALTHOLD/COURSE_HOLD) and re-entering WP mode resumed the arc with stale geometry, banking toward an outdated out-leg. State is now file-scope (fwArcEngaged, fwArcPrevLegBearing) and cleared on controller reset; the roll S-curve smoother is re-seeded the same way (fwRollSmoothReseed) so stale filter state cannot fire a spurious ramp. - A leg change while an arc was still active (short legs: FLY_BY early-reach advances the mission mid-turn) was silently consumed: the arc completed onto the stale out-bearing and the new corner got no coordination at all. Now the closed-loop capture is retargeted onto the new leg (bounded +/-phi_nom, hands back once aligned), so quick consecutive corners degrade gracefully instead of being skipped.
…ual throttle Maintainer decisions 2026-08-20 after the resume code review: nav_fw_wp_turn_max_lead_time: min 0 -> 1000ms (0 silently disabled the FLY_BY anticipation: lead distance 0 -> WP reached by proximity radius, arc engages uncapped at the corner - an undesigned mode). Stays a permanent user setting instead of being hardcoded before release; DEV note dropped, Settings.md regenerated. Energy bank guard: evaluate the near-throttle-limit branch against the AUTO throttle demand (before allow_manual_thr_increase is added). Pilot-held full throttle permanently armed the branch even though the autopilot still had throttle authority; the throttle branch now cleanly means 'auto-throttle authority exhausted'. A genuine energy crisis is still caught by the OR-connected pitch branch (climb pitch saturates).
The arc coordinator's original exit (design v1) handed control back to the heading PID at a fixed heading error (nav_fw_wp_turn_handback_angle, 15 deg) with a 300ms progress guard against premature handback on cog noise (NAV_FW_ARC_HANDBACK_GUARD_MS). Both became obsolete when the exit was replaced by the closed-loop capture phase (bank proportional to remaining heading, cannot hand back early or overshoot) but survived as dead code: the guard define was never referenced, the setting was stored but never read. Remove both (PG_NAV_CONFIG 11 -> 12 for the struct change) and skip the turn feed-forward computation entirely while the arc coordinator drives the roll: its result was discarded (the arc bank command already is the coordinated bank), and its arc branch only fed a debug channel with values that were never applied. fwArcDir is unused after that and removed.
…edictive roll-out lead - loiter FF only once established on the circle: fed during the (much larger) approach cone it fought the approach carrot and slewed the entry across the circle - capped and >150 deg corners fly the bounded closed-loop capture instead of the reactive PID / a degenerate tangent circle (flutter) - capture leads the roll-out by omega*(tau + control_ease), tau from the angle-P gain: the airframe sheds bank slower than the command falls, the residual turn rate was overshooting the out-leg - keep the roll smoother's reseed baseline current while the arc drives (a stale reset-time seed caused a brief roll twitch at arc handback) - nav_fw_wp_turn_max_lead_time bounds/default now 3000/6000/12000 (the 3 s ceiling capped nearly every cruise-speed corner) Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
- FLY_OVER now engages the arc coordinator as a reverse FLY_BY: the turn circle is pinned at the overfly point (led by the roll-in drift) and the exit course is the tangent from that circle through the next waypoint, so the roll-out lands exactly on a straight line to it. Handles any turn angle including full reversals; HITL: exit course within 2 deg of the direct line at all tested corners - shaped roll-out for all arc exits: the capture command's collapse is rate-limited to the entry ramp's build-up rate (phiNom/tEase) with the no-overshoot envelope kept on top; the lead gains the ramp's heading share (0.5*omega*tEase) and STEADY hands over early enough for the ramp to fit; handback waits until nearly level - debug: ch2 = active exit course while the arc runs Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
b14ckyy
changed the title
|
I did some HITL testing on this and it does seem to be a noticeable improvement on the current method. However, I did notice that sometimes it behaves unpredictably flying off in a direction that doesn't make sense. It does always recover back to the next WP but something doesn't seem quite right. Needs more testing to better understand when this happens. Other times it seems slow to correct back to the WP course line when WP tracking is ON. It'll fly parallel to the course line for some time at some distance away then suddenly decide to head toward the course line as you'd expect. Almost as if something isn't triggering the way it should. The other more significant issue is shaking/bouncing in roll usually when manoeuvring near a WP. Looking at logs there are high amplitude rcCommand(roll) spikes which probably account for the shuddering you see visually. #11804 has removed the |
|
Do you have a log of that behavior and maybe the WP mission you tested and Weather parameters? A few notes to the things you noticed but its guesswork without more details. The parallel straight before correcting with path tracking: The Fly-Off: Shaking/bouncing: PS: |
|
Both circle-line intersection solves in |
|
Sounds reasonable. 68 bytes are 68 bytes. Will do and also fix the conflict and look at breadoven's twitching issue. |
…-predictor Sync with maintenance-10.x after the MAVLink guided-mode work landed (iNavFlight#11717, iNavFlight#11731 et al.). Conflict in src/main/navigation/navigation_fixedwing.c, calculateVirtualPositionTarget_FW(): upstream iNavFlight#11717 changed the loiter radius source to navigationGetLoiterRadius(), which adds a GCS loiter radius override (POSHOLD states only); this branch wraps the same spot with the stable-loiter-radius peak hold. Resolved by keeping the stable-radius block and feeding it navigationGetLoiterRadius() as the base radius, so the GCS override is honored underneath the peak hold. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Review request: both circle-line intersection solves in updateFwTurnArc() called raw sqrtf(); navigation/ convention is fast_fsqrtf(), which maps to the CMSIS VSQRT path on ARM targets. No behavior change - both discriminants are checked > 0 before the call. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
|
I did some more HITL testing and worked out the roll shaking is caused by However, even after fixing the above there was still a lot of spiking in rcCommand(roll) albeit at a reduced amplitude so it's not that visually obvious. On checking further though this turns out to be caused by the The only odd looking flight path I saw this time can be seen in the following screenshot after waypoint 3. This is with wind (23kts N maybe), WP tracking accuracy OFF and I think the WP turn mode was set to 
|
|
Thanks a lot that helps. I also did some new logs with different settings and was able to to replicate some rough angle changes that should not happen. Claude analyzed the code based on your previous description and also found some candidates but I will compare with high rate logs right now. the odd looking path at WP3 is exactly what I meant. There is no wind taken into account yet. the turns are planned by ground speed. Plus: the strong wind from north must have turned your heading so far north, that it tripped the fallback function where it will basically do a fly-over if the turn is over 160° or the time to the turn start is above default 10s from the actual waypoint. That is normal at almost 180° turns as cutting the corner into the new path is literally impossible. Will keep you updated with a new build after testing here. thanks a lot for your help with this. |
|
Thanks — both of your findings check out, and the HITL logs let me pin down the mechanism. Logging at 500 Hz onboard (SD card), so no aliasing. Root cause of the roll shaking: the arc seams, not the arc phases. Claude response: Additionally: Probably also a tuning thing and in reality the planes are not as floaty as in X-Plane so that should be save. |
HITL logs at 495 Hz showed the roll shudder was not in the arc phases but at the transitions between them, where a shaped command was replaced by an unshaped one in a single nav update. - Arc hand-back: the arc releases with the wings near level while the PID and turn FF still see the full carrot error (measured 21 deg while the arc's own exit error was 1.7 deg), so their command was adopted as a step of -1818 cd. Crossfade from the arc's last command over the turn ease time, edge-triggered so every release path is covered, and independent of nav_fw_control_smoothness - that defaults to 0, which makes the S-curve smoother a pass-through. - ARC_RAMP_IN -> ARC_STEADY: the ramp ends on phiNom while the steady law recomputes from phiLive plus the accumulated eR/eH. Measured steps of +453 to +2000 cd. The steady law now blends from the ramp's final command. - ARC_STEADY had no slew limit at all, unlike the other two phases. On a tight arc the tangent bearing is ill-conditioned near the centre and eH inverted between two updates, commanding +8140 cd into the bank limit and saturating rcCommand(roll). Two-sided rate limit added. - Turn FF: heading error and velXY arrive at GPS rate (5 Hz in HITL) while the controller runs at 50 Hz, so the FF was a staircase stepping 250-600 cd every 200 ms. Slew limited rather than filtered, so a settled command is not lagged. The taper now also starts at the deadband edge instead of at zero, removing the step at the gate. - The away arc of the S sequencer blocks the hand-back and stranded the aircraft for 39 s with path tracking suppressed when the second-arc pickup never triggered. Bounded by the time to fly half the away circle. - nav_fw_land/turn hand-off threshold raised from 1.5 to 3 deg: over 13 logged turns the 1.5 deg gate was never satisfied before release, and the residual bank is bounded by the separate bank gate, not by this one. - Debug: clear the phase and heading-error channels on release, and show the pickup along-track distance while the away arc runs. Stale channels previously read as arc episodes minutes after the arc had ended. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
|
Alright, last commit should now fix the transition from loiter-WP into the next track with Path tracking enabled and not swerve multiple times anymore. Also wind induced turn offsets should now hand over to path tracking MUCH earlier right after the ease out. Wind gust induced offsets from ease in -> arc and arc > ease out should now always be corrected softly. I will do more HITL testing before the real flights this weekend. Hope the predictions stay. @sensei-hacker technically its safe to use. If you want to merge I can tweak any remaining issues during RC phase as well. #11804 should be merged beforehand though if you want to have both features separated but its also included here if you want to have it in one. Just tell me what you prefer. |
nav_cruise_lock_on_level makes the roll-out gate optional, so with it off the course locks immediately while the aircraft is still turning - the case the 'cog - gyroRateDps(YAW)' lead was there for. Restore it for that path; the gated path locks after the roll-out and the forced timeout lock does not need it either (2.5 s of level command means level or already in trouble). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
|
cherry picked a missing commit from #11804 to avoid merge conflicts/misses |
|
I did another HITL test, mainly related to checking D term using measurement rather than error, and found a good example of an "errant" flight path at WP 5, shown in attached screenshot. For some reason it decided to do a turn reversal. This was no wind with tracking accuracy ON. I did also notice the flight path was snaking a lot more than before, e.g. between WP 1 and 2. Don't know if this is due to the D term change or the issue mentioned in the comment above. Pretty sure I noticed this turn reversal at a WP turn when I tested before without the D term changes though. The D term change does eliminate the PID output spikes and in fact using measurement makes sense given error can change abruptly when 
|
|
@breadoven thats strange and I have never seen that before. Can you send me that mission and blackbox please? No idea how to replicate that. PS: can you please compare my tune with yours? EDIT2: whats your WP radius btw? I have a suspicion. that should be set to 10m for fixed wings until a future change I have in mind is in. |
|
I've attached the log although it only includes debug_always debugs not the ones from this PR. However, the debugs are useful and defined as follows (all in 0 = The first log is the one with the turn reversal and no wind. The second is with wind of around 30 kts although the reversal didn't happened in that case (it flew WP 5 perfectly in fact). The mission is also attached. It's an old autospeed test mission but in this case the speeds are all set 0 to avoid complicating things. I think the snaking might be caused by control smoothing being set as it was before (value of 7). After the recent changes I made it might need adjusting. If you look at the log between WP 1 and 2 debug 4 shows nav_wp_radius = 300. inav_001.zip EDIT: sorry I got confused over which Log had the turn reversal, seems it was second log with wind. But I guess you already worked that out ! |
The missed-waypoint bearing check compares the bearing to the waypoint against the inbound leg bearing. Once the aircraft is established on the outbound leg that angle settles at (180 - turn angle), so beyond a ~80 deg turn it can never reach the 100 deg limit - it can only fire on the brief swing right next to the waypoint. In a HITL log of a 114 deg turn that swing peaked at 99.4 deg and missed by 0.6 deg. The waypoint then stayed active behind the aircraft, which correctly steered back to it: a full reversal 81 m past the waypoint before the check finally tripped. With nav_wp_radius small (3 m there) the radius path cannot cover this either, and FLY_OVER/FLY_INTO never set wpTurnSmoothingActive, so the anticipated-turn path does not apply. Treat the waypoint as passed once the aircraft crosses the plane through it normal to the inbound leg. Independent of turn angle and of cross-track offset, and it cannot fire early: at activation the aircraft sits a leg length short of that plane. Scoped to airplanes in WP mission mode, so RTH, trackback and the autoland approach are unaffected; FLY_BY returns earlier via wpTurnSmoothingActive and never reaches it. Verified against the logs: fires 0.1 m past the plane (6.95 s and 81 m earlier than before) on the affected turn, and never fires on a clean FLY_BY run. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
|
Found it — it's not the turn coordinator. WP5 was flown correctly: passed 5.1 m from it, on the leg line, heading 303° vs a leg bearing of 295°. But it was never registered as reached, so it stayed the active waypoint and the controller correctly steered back to a waypoint behind the aircraft. That reversal is the loop in the track. In the log All three detection paths missed:
So beyond ~80° of turn that check structurally cannot fire in steady flight. Upstream masked this because turn smoothing dropped the limit to 60°; that reduction was lost when Fix: treat the waypoint as passed once the aircraft crosses the plane through it normal to the inbound leg. Independent of turn angle and cross-track offset, and it can't fire early — at activation the aircraft is a leg length short of that plane. Scoped to airplanes in WP mission mode, so RTH, trackback and autoland are untouched; FLY_BY returns earlier via Worth raising This Commit is UNTESTED at this point! I will run more tests with similar edge cases later this evening. But your extreme wind condition test was definitely helpful. |
|
I am testing already with long flights and different WP modes. Also with insane high 30kn wind speed. So far no missed WP at all. its only the transition from the alignment turn into path tracking thats still not clean. Working on it. |
The into-turn corner cut ends on the outbound leg without crossing the passage plane through the waypoint - beyond a 90 deg turn it even recedes from that plane, so no geometric reached-check can fire. The old carrot then steers back toward the previous leg after hand-back until a recovery arc undoes the excursion (HITL: two of three laps at the same WP, FF pinned at full bank for 2.6-3.3 s; the third lap escaped by chance). Mark the WP reached when the main arc commits onto the outbound leg, via the same wpTurnSmoothingActive path FLY_BY has always used at turn start. State-based, so it fires at any lateral offset. Guard the mid-arc retarget so the mission advance this causes does not degrade the running arc to a bare capture; gated to FLY_INTO because FLY_OVER shares the pickup but has already advanced its WP at the overfly. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
25 Hz HITL long-runs across all three WP turn modes show the residual hand-back swings come from command amplitude, not steps - rate limiting had stretched them without removing them. - Turn feed-forward: navHeadingError measures to the path-tracking carrot, so a lateral offset after a completed turn re-entered the FF at up to full bank for a plain leg capture (reproducible per waypoint to a few centidegrees), snaking every transition onto the leg. Arm the FF on a leg change and disarm it once aligned or when the arc flies the turn; leg changes the arc consumes itself do not re-arm. - Capture law: slew-limit the rise like the decay (it stepped to full bank at capped engages and mid-capture mission advances) and seed a fresh engage from the live nav command instead of zero. - RAMP_IN: seed the ramp from the live nav command as well - engaging out of a banked loiter dipped the command to zero before rebuilding (-3285 cd in one update at 50 m/s, a visible roll twitch). - Loiter exits: the loiter-time reset wipes the leg reference every cycle, so a WP advance landing on the first tracked cycle was invisible to the leg-change detection and the exit never engaged an arc - the raw FF then flew the whole turn. Unseeded plus grossly off the leg course now counts as a leg change. - Set arcR in the capture-only engage; it sizes the away-arc timeout. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
|
Alright I think I now have all that was left. In addition a second commit with multiple small fixes and tweaks to get rid of all the situations that could cause sudden heading errors that show as roll twitches. Tested under extreme wind conditions like @breadoven
Now WP Turn Smoothing should do a proper job with the rest (direct fallback) and anything else is limited by the not yet implemented wind awareness and the way path tracking works. But both don't belong in this PR. |
…-smoothing Brings in the VTOL auto-transition ownership rework, terrain support and the 9.1 back-merge. Two conflicts in navigation.c: - Adjacent #define blocks: kept both (course-lock constants + the new USE_AUTO_TRANSITION MIXERAT retry constants). - PG_NAV_CONFIG version: upstream made it conditional (11 with USE_AUTO_TRANSITION, else 8); this branch had bumped 8->9 for its added fields. Resolved one above upstream in both variants: 12 / 9. Stored nav settings reset to defaults on this bump. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Restack on the roll-smoothing branch freshly merged with upstream maintenance-10.x (VTOL auto-transition rework, terrain, 9.1 back-merge). navigation.c resolutions (both define blocks, conditional PG_NAV_CONFIG 12/9) carried over cleanly from the fw-roll-smoothing merge. Resolved here: DEBUG_FW_TURN moved behind the new DEBUG_VTOL_* entries in debugType_e, debugModeNames and the settings.yaml debug_modes table (order is wire-visible, upstream entries keep their indices); Settings.md regenerated with update_cli_docs.py. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
|
I did some more HITL testing with the latest changes and it's certainly better where the snaking flight path is concerned. However, the thing I did find is it's over correcting after turning at a WP causing it to overshoot onto the other side of the course line. This can be seen in the following screenshots. Doesn't seem to be such a problem with The other thing that I realised is ... how is COORD_FLYBY COORD_FLYOVER |
This is 100% a bad tune (FF and Rates and/or too much bank angle) for the HITL what I was talking about earlier. The ease in and ease out with its ramped turn rate is pre-calculated. If the actual rates are too much off the setpoint rate, it will either roll slower than the FC anticipated or faster and both can cause overshoots like that. Nothing I can do about it. EDIT: This overshoot behavior you see has btw cost me 2-3 days of work and HITL testing to fix until I did actual Debug logging over MSP to find the cause, just to have Claude tell to my face that my Tune is shit! And he was right. Autotuned until it felt right and the issue was gone.
the difference is how the turn is flown towards the new waypoint or track. Legacy mode goes into a full nav bank angle and turns by best effort. So against the wind very tight, with the wind much longer and in crosswind all different shapes with a teardrop end. COORD_FLYOVER does a smooth circularized turn based on speed so its much more predictable with the same roll rate for ease in and ease out every time. This controlled turning is especially important for mapping tasks or gimbal use where you have consistent and reproducible roll behavior over the course even with different wind conditions (up to a reasonable limit). |
|
Have you guys tested this with a plane that uses a means of yaw control to also assist turning when I had wrote some setup info here that gives insight into the settings that will cause a plane with active yaw control to go into a spiral dive. If those setting are increased too much or incorrectly tuned. |
|
@Jetrell Prepairing my Ranger 2400 for tomorrow and will enable yaw control for that |
5 participants
Summary
FW waypoint turns are currently flown reactively: the heading PID chases a moving carrot, so
every corner is a late, speed-dependent overshoot followed by a correction back to track. This
PR plans the turn instead: from the aircraft's actual coordinated-turn radius
R = V²/(g·tan φ)(speed andnav_fw_bank_angle), each corner is constructed geometrically andflown as an explicit coordinated arc — smoothstep roll-in, steady arc with live feed-forward plus
radial/tangent feedback, and a predictive roll-out that hands back aligned and level. Loiter
circles are held by the same steady law once established.
Detailed technical information:
coordinated-turns-overview.html
Companion PR: iNavFlight/inav-configurator#2711
Note: branch is stacked on
fw-roll-smoothing(triggered S-curve roll smoothing + cruisecourse lock). Either merge that first and rebase, or include it here — maintainer's call.
What it does
Turn modes (one consolidated setting,
nav_fw_wp_turn_mode):R·tan(Δ/2)plus aroll-in lead, so the arc joins the next leg tangentially; the WP is passed abeam.
line to the next WP. With
nav_fw_wp_tracking_accuracyenabled it instead flies abounded-intercept S (intercept ≤ 45° to the new leg, reverse-roll straight, corner-cut arc)
that rolls out ON the new leg.
arc pinned at the WP cross the waypoint already aligned with the outbound course.
Robustness
tracking enabled the capture steers onto the leg line (tapered intercept, capped at
nav_fw_wp_tracking_max_angle) instead of ending parallel to it a turn-diameter off.continuous capture; missions continue without error (HITL-verified).
Loiter
replaces the carrot PID as a latch and regulates the radius directly (the pursuit carrot
plus any additive feed-forward always settles inside the commanded circle). Approach and
entry stay on stock guidance.
per-revolution peak hold (ratchet up instantly, decay slowly) — no more "kidney" deformation
in wind; the circle widens smoothly at speed and migrates back down afterwards.
Bank semantics: target vs. ceiling
nav_fw_bank_anglechanges meaning: it is now the sustained target bank — the value allturn and loiter radii are planned for — not an absolute limit for nav modes. Corrections
(holding an arc against wind, capturing the exit course) may exceed it temporarily.
max_angle_inclination_rllremains the absolute hard ceiling; no nav command everexceeds it. The reserve between target and ceiling is what makes the planned geometry
robust: the radius is sized for a bank the aircraft can always deliver, with headroom left
for corrections.
Energy bank guard (introduced to make that headroom safe)
commanded climb near the pitch/throttle limit can stall while the reserve is being used.
When a banked climb stagnates, the effective bank ceiling eases down toward (and if needed
below) the planning target — the turn widens, the climb recovers. After a hold-off it
gradually releases back to the hard ceiling.
the PID clamp and the planning bank itself (a reduced ceiling also widens newly planned
radii). Manual throttle raises are respected as recovery input.
use the FLY_BY construction.
Settings
nav_fw_wp_turn_modenav_fw_bank_anglemax_angle_inclination_rllstays the absolute ceilingnav_fw_wp_turn_max_lead_timenav_fw_turn_ff_gainnav_fw_wp_turn_control_easeMigration:
PG_NAV_CONFIGis version-bumped — nav settings reset to defaults on first boot.Debugging
debug_mode = FW_TURNexposes the full system (radius, coordinator state machine, exit course,bank command, FF, guard ceiling, ease time) — documented in
docs/development/fw-turn-debugging.md.docs/Fixed Wing Landing.mdupdated (approach turningpoints now derive from the coordinated turn radius).
Validation (HITL: X-Plane, MATEKF765, MSP debug telemetry)
correlate with the (uncompensated) wind vector estimated from the logs.
reversal. Tracking-ON S: 14/14 corners at 0.5–6.5 m from the leg wind-off; worst 9.1 m at
≈15 km/h wind.
offset and a late 46° tracker correction).
radius migration (previous additive-FF experiment settled 28 % inside the commanded radius —
replaced by the circle controller).
Known limitations / follow-ups
shifts the effective radius around the turn (±3–9 m residuals at ~15 km/h wind).
Wind-vector compensation is planned as a follow-up PR (10.1).
nav_fw_turn_ff_gainandnav_fw_wp_turn_control_easestay CLI-adjustable until the systemis field-proven across airframes; both are candidates for hardcoding in a follow-up.