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 7 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 245 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.
|
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. |
😄 But I just call him Becky |
|
Shut up and merge xD <3 |
|
@sensei-hacker |
All done testing? |
|
@error414 is still on it. |
I had a quick look at #11812 in HITL and I found the same problem in CRUZ. This was with a 20kt wind, probably a crosswind where the issue occurred. With yaw stick centred the plane ended up drifting in yaw with a > 10 deg bank and didn't seem to want to lock on to the course or return to level. You had to adjust course using the yaw stick again and eventually it rolled level and maintained course. I need to check it again just using the changes in this PR to see if it is repeatable I guess. |
|
Iinteresting. In my HITL tests I flew in 30kph wind with +10kph gusts at 60-70kph cruise. Never had the plane not center. Do you use the default tune from the HITL plugin or did you re-autotune? because the stock tune on github does not work anymore. Had to retune it to fly somewhat acceptable. But I can add a timeout. on stick release a 2500ms timeout to lock in the course should be fine as a fallback. |
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>
|
added the 2.5s timeout and #11812 is also updated with the same change. |
|
The drift issue is quite easy to replicate in fact using just the changes in this PR. Just fly a 360 turn using max yaw stick in CRUZ and it fails to roll back level. It's the roll issue that's the problem obviously, ends up stuck on 20 degs of roll. I'd have thought this indicates a bug somewhere more than anything else, something isn't resetting correctly. Relevant parts of the Diff are: |
|
I see what you mean. I an replicate it as well. It seems like the longer the yaw turn takes, the more the resulting bank angle deviates at the end. Until it never passes the 10° to relock. I will have a look as I am not sure if that bank angle thing is an old bug (that never triggered due to the course lock on center) or on my end. if its an old bug it should be a separate PR. |
|
I'm going to test this PR on field, should I wait for fix? |
|
Found it! The Nav TurnPID Integral was never reset after a build up and kept the bank angle. Fix in the works and will test locally first. @error414 no need to wait. its not crashing your plane. just know that after a LOOOONG yaw cruise turn (10s+) the plane might keep turning until you slightly counter control. Roll cruise turn not affected and the coordinated turn PR is also not affected since they reset the nav PID properly. |
|
ok, I'm not worried about crashing, but if test will help you. |
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>
|
Fix tested and confirmed. Good catch @breadoven! I am not used to use yaw stick to turn in cruise. Tested it but never long enough turns to wind up the Nav Integral over the threshold to trigger that. Now clean exit in HITL. |
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>
5 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