AC_PosControl: compensate for velocity FF when initializing controller#32758
Open
lthall wants to merge 1 commit into
Open
AC_PosControl: compensate for velocity FF when initializing controller#32758lthall wants to merge 1 commit into
lthall wants to merge 1 commit into
Conversation
lthall
force-pushed
the
20260330_FF_Init_Compensation
branch
from
315801c to
39ed4ac
Compare
Contributor
There was a problem hiding this comment.
Pull request overview
Note
Copilot was unable to run its full agentic suite in this review.
Adjusts AC_PosControl controller initialization to avoid a first-cycle transient by compensating velocity feed-forward during NE/D init, and simplifies integrator initialization accordingly.
Changes:
- Compute initial
_vel_target_*so velocity PID P and FF terms cancel on the first cycle (NE and D init paths). - Simplify NE velocity PID integrator initialization to use
_accel_target_*directly (removing FF-related compensation).
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+560
to
+563
| // Set velocity target so that P and FF sum to zero on the first cycle: | ||
| // 0 = (target - measurement) * kP + target * kFF | ||
| // target = measurement * kP / (kP + kFF) | ||
| _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy() * (_pid_vel_ne_m.kP() / (_pid_vel_ne_m.kP() + MAX(_pid_vel_ne_m.ff(), 0))); |
| // Set velocity target so that P and FF sum to zero on the first cycle: | ||
| // 0 = (target - measurement) * kP + target * kFF | ||
| // target = measurement * kP / (kP + kFF) | ||
| _vel_target_ned_ms.z = _vel_estimate_ned_ms.z * (_pid_vel_d_m.kP() / (_pid_vel_d_m.kP() + MAX(_pid_vel_d_m.ff(), 0))); |
| // Set velocity target so that P and FF sum to zero on the first cycle: | ||
| // 0 = (target - measurement) * kP + target * kFF | ||
| // target = measurement * kP / (kP + kFF) | ||
| _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy() * (_pid_vel_ne_m.kP() / (_pid_vel_ne_m.kP() + MAX(_pid_vel_ne_m.ff(), 0))); |
| // Set velocity target so that P and FF sum to zero on the first cycle: | ||
| // 0 = (target - measurement) * kP + target * kFF | ||
| // target = measurement * kP / (kP + kFF) | ||
| _vel_target_ned_ms.z = _vel_estimate_ned_ms.z * (_pid_vel_d_m.kP() / (_pid_vel_d_m.kP() + MAX(_pid_vel_d_m.ff(), 0))); |
rmackay9
reviewed
Apr 20, 2026
| // Set velocity target so that P and FF sum to zero on the first cycle: | ||
| // 0 = (target - measurement) * kP + target * kFF | ||
| // target = measurement * kP / (kP + kFF) | ||
| _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy() * (_pid_vel_ne_m.kP() / (_pid_vel_ne_m.kP() + MAX(_pid_vel_ne_m.ff(), 0))); |
Contributor
There was a problem hiding this comment.
We should probably ensure we don't get a divide-by-zero if (_pid_vel_ne_m.kP() + MAX(_pid_vel_ne_m.ff())) is zero
Contributor
|
This looks fine to me once we add a check to avoid divide-by-zero. It would be nice to see some testing results showing the jump/no-jump before vs after. I've added @bnsgeyer in case he has a moment to review this. |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Adjust the velocity target during NE and D controller initialization so the velocity PID P and FF terms sum to zero on the first cycle, eliminating a transient bump on controller init
Simplify integrator initialization by removing the now-unnecessary FF compensation from set_integrator
Classification & Testing (check all that apply and add your own)
SITL only.
Description
When a position controller is initialized (e.g. on mode switch), the velocity target was set directly to the current velocity estimate. With a non-zero feed-forward gain, this caused a net output on the first cycle because P * (target - measurement) is zero but FF * target is not, producing a transient.
The fix sets the velocity target to:
target = measurement * kP / (kP + kFF)so that P * (target - measurement) + FF * target = 0 on the first cycle. This is applied to both the NE (horizontal) and D (vertical) init paths.
Since the velocity target now correctly accounts for FF, the integrator init simplifies to just the current acceleration target without needing to subtract the FF term.