FW nav: replace roll PT1 smoothing with a triggered S-curve (no steady-state lag) - #11804
FW nav: replace roll PT1 smoothing with a triggered S-curve (no steady-state lag)#11804b14ckyy wants to merge 4 commits into
Conversation
|
ⓘ 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 QodoFW nav: step-triggered roll S-curve smoothing + delayed CRUISE course lock
AI Description
Diagram
High-Level Assessment
Files changed (4)
|
Code Review by Qodo
1. Course lock unit mismatch
|
![qodo-free-for-open-source-projects[bot]](https://avatars.githubusercontent.com/in/1068000?s=60&v=4){kind=small}
…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.
f6ca64d to
4c9288a
Compare
|
Test firmware build ready — commit Download firmware for PR #11804 244 targets built. Find your board's
|
|
RAM / Flash usage vs. base branch — commit
See RAM/flash optimization guide for techniques to reduce usage. |
…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.
Resource cost vs
|
| Target | Flash Δ | RAM Δ |
|---|---|---|
| MATEKF405 | +256 B | +24 B |
| MATEKF722 | +384 B | +32 B |
| MATEKF765 | +384 B | +24 B |
| MATEKH743 | +384 B | +32 B |
(measured with arm-none-eabi-size: flash = text+data, RAM = data+bss)
|
Hmm something I'm not sure aobut:
Centering the stick in COURSE_HOLD sets the course. Yeah I'm not sure that's a bug? |
|
The problem is that when you are in a CRSH or CRUZ turn with roll or yaw stick and you quickly release it, the plane is still at full bank. So it continues to turn for a few degrees before it levels off. that means it ALWAYS overshoots the new set COG at stick center time and makes an S-turn correction the other way. Even worse, if you enable cruise mode out of a sharp turn in angle mode or WP mode, the plane is all over the place going back to the ground course the plane had at the time of mode enabling. Roll smoothing boosts this behavior even more (also the old PT1 method). Locking the course at level off instead is fully adaptive and always guarantees a solid flight that only does commanded maneuvers and no weaving. Plus it prevents the plane from permanent S-Turns if the Roll FF is overturned as it won't start to hunt its own nose. |
|
But if we make that change the docs need an update I just realize. If you give the OK I will do that |
Yes, agreed - if you're in a steep bank and center the stick, the plane will continue to turn briefly. Always, meaning we can't change that, agreed. What is proposed here is to make it continue to overshoot the course that the pilot requested? I'm not clear on why that's a great idea. |
|
It makes it more intuitive and nicer/smoother to fly. If you fly a real plane and you want to fly towards heading 120° you also don't keep full stick until you reach 120, then start to level off and turn back to actually hit 120. Right? So with the change the pilot still can ease down the bank turn rate and level off exactly where he wants to go and if its a snap release, its whatever the plane ends off when level. And then stays on that course |
|
Compromise: I make it a setting |
This would be a better idea because one of the reasons for #10187 was to stop the problem of the course drifting away from the course at stick centre making it awkward to get the plane pointing where you want it. Although admittedly that was largely down to the fact that the controller wasn't being reset after adjustment using the Yaw stick prior to 10187. |
|
Alright I will make it settings gated. With Stick Center lock, your #10187 still matters. Roll turns in cruise will still overshoot and back-correct. With Level-Lock both control options will be steady and lock course exactly when levelled off. |
|
Setting implemented. Configurator PR open and will commit firmware side when tested. |
…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>
|
Setting works as expected. Default is the new behavior, can be turned off for old behavior. Hope thats okay. level_lock.mp4 |
|
Hey Mark, I was checking this PR against my terrain following PR (#11785 lives in 3D Cruise for now, so I wanted to be sure the new course-lock logic doesn't interact with it - it doesn't, great :)) ) and one thing popped out - the lock-on-level wait has no timeout. If the plane stays banked over 10dg for a while (wind gusts, bumpy air around the threshold, or just a badly trimmed plane, the angle loop doesn't fully level), the course is never locked - Cruise follows the COG with zero roll command and quietly stops holding a course, while still showing CRUZ. It doesn't seem dangerous to me (well i might be wrong, so I won't conclude that strongly), but the pilot can't tell. Maybe lock anyway after 2–3 s as a fallback won't hurt anyone. |
|
Hey, its Marc ;) and as long as there is an error in the angle, the Angle P control will keep pushing and the roll PID controller will push harder and harder until it goes level. if something on the plane is so broken that it will be stuck outside of that 10° threshold, then you should not even fly in a nav mode and fix it. |
|
That makes sense, was focused on the lock logic itself and forgot the angle loop is pushing toward level the whole time. |
4 participants
Summary
nav_fw_control_smoothnesscurrently applies a PT1 low-pass to the fixed-wing nav roll command. That buys smoothness at the cost of a permanent, uncompensated lag between what the navigation controller commands and what gets executed: every course correction is delayed — including during steady tracking, where no smoothing is needed at all — and the lag grows with the smoothness setting.This PR replaces the roll-axis PT1 with a triggered S-curve easing:
control_smoothness-derived window (n × 100 ms, 0 = off, capped at 900 ms), then passes the command 1:1 again — zero added lag in steady tracking.Because the smoothing is purely time-based, behavior is airframe-independent: aircraft whose natural roll response is slower than the easing window simply won't notice it.
gyroRateDps(YAW)lead term mixed a rate (deg/s) into an angle — effectively a fixed one-second yaw lead. With the bank gate the residual turn rate at lock time is negligible, making the term obsolete.control_smoothness≤ 9 (the previous 1000 ms cap was unreachable).Related fix: cruise course lock only once rolled out
Flight testing the smoothing surfaced a longstanding COURSE_HOLD/CRUISE quirk that softer roll-outs make more visible: the course is locked the moment the stick is released or the mode engages — while the aircraft is still banked. It then keeps turning through the level-off, overshoots the locked course and flies a reverse correction turn.
The second commit delays the course lock until the bank is below 10°; until then the course follows the actual COG. This covers all three lock paths (roll-stick release, yaw-stick release, and a banked mode entry such as switching out of an RTH turn into CRUISE). 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. Fixed-wing only; multicopter course hold is unchanged.
What does NOT change
control_smoothness = 0(default) the roll command path is a pure pass-through.Context
This is groundwork for an upcoming fixed-wing coordinated-turn / turn-predictor PR: a predictive turn controller must know its own roll-in dynamics, which an output low-pass hides. Splitting this out keeps that (much larger) PR reviewable and lets this behavior change be evaluated on its own.
Testing
Settings.mdregenerated;nav_fw_control_smoothnessdescription updated to match the new mechanics.Demo: Smoothing OFF, Level 5 (500ms) and Level 9 (900ms). RTH Engage, Roll turn, yaw turn.
smoothing.mp4