Use Measurement For Fixed Wing Position PID D Term

[breadoven]

Changes fixed wing position PID D term from using error to measurement.

Reason - Error is based on the virtualTargetBearing which can change abruptly when target position changes resulting in large D term spikes. Measurement is course over ground which changes consistently with aircraft movement resulting in consistent changes in D term without spikes.

Course over ground (CoG) estimate also changed to smooth the CoG output reducing D term spikes. CoG is now only updated when the position estimate is published to Nav. This removes unnecessary recalculation of CoG at loop rate and also allows smoothing by use of filtered Vel x and Vel y.

HITL testing shows D term spikes are pretty much eliminated during WP missions which is where changes in virtualTargetBearing tend to occur most often. Hard to say if there is a noticeable improvement in flight control but it may help improve servo life.

[qodo-code-review]

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

[qodo-free-for-open-source-projects]

PR Summary by Qodo

Use course measurement for fixed-wing position PID D-term

🐞 Bug fix 🕐 Less than 10 minutes

AI Description

• Derives fixed-wing position D-term from course over ground instead of heading error.
• Prevents abrupt virtual-target changes from producing derivative spikes during waypoint missions.

Diagram

graph TD
  VT["Virtual Bearing"] --> HE["Heading Error"] --> PID["Position PID"] --> ROLL["Roll Command"]
  COG["Course Ground"] --> PID

Loading

High-Level Assessment

The following are alternative approaches to this PR:

1. Smooth virtual target changes

• ➕ Retains derivative-on-error behavior
• ➕ Could reduce discontinuities for other heading-error consumers
• ➖ Introduces navigation response lag
• ➖ Requires additional filter tuning
• ➖ May reduce rather than eliminate derivative kick

Recommendation: Use derivative-on-measurement as implemented. It is the standard way to avoid setpoint derivative kick, requires no new tuning mechanism, and isolates the change to the affected fixed-wing position controller; virtual-target smoothing is broader and may delay waypoint response.

Files changed (1) +1 / -1

Bug fix (1) +1 / -1

navigation_fixedwing.cDerive fixed-wing position D-term from course measurement +1/-1

Derive fixed-wing position D-term from course measurement

• Removes the PID_DTERM_FROM_ERROR flag from the fixed-wing position heading controller while preserving conditional integrator shrinking. The shared PID therefore computes the derivative from course over ground, avoiding spikes when virtual target bearing changes abruptly.

src/main/navigation/navigation_fixedwing.c

[qodo-free-for-open-source-projects]

Code Review by Qodo

🐞 Bugs (0) 📘 Rule violations (0) 📎 Requirement gaps (0) 🎨 UX issues (0) 🔗 Cross-repo conflicts (0) 📜 Skill insights (0)

1. Wrapped COG spikes D-term ✓ Resolved 🐞 Bug ≡ Correctness

Description

Without PID_DTERM_FROM_ERROR, navPidApply2 differentiates raw course over ground, so a normal
359.99°→0° crossing is treated as an approximately −360° step and produces a large roll-command
transient. The fixed-wing navigation D gain is enabled by default, and output clamping only turns
this spike into a maximum-bank command rather than preventing it.

Code

src/main/navigation/navigation_fixedwing.c[555]

+    const pidControllerFlags_e pidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;

Evidence

The controller now selects measurement tracking, whose implementation subtracts consecutive
measurements directly. Navigation supplies COG as a value wrapped to one revolution, while the
configured fixed-wing XY D gain is nonzero, proving that a north-boundary crossing reaches the
active D path; the resulting controller output is merely filtered and bounded before becoming the
roll adjustment.

src/main/common/fp_pid.c[60-78]
src/main/navigation/navigation_fixedwing.c[525-528]
src/main/navigation/navigation_fixedwing.c[555-567]
src/main/navigation/navigation_pos_estimator.c[742-752]
src/main/fc/settings.yaml[2230-2246]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
The new derivative-on-measurement path differentiates wrapped COG directly. Crossing 0°/360° therefore creates an artificial full-circle delta and a large D-term roll correction.
## Issue Context
`navHeadingError` is wrapped safely, but `actualState.cog` is represented as a wrapped angular value. Preserve derivative-on-measurement while making the measurement delta wrap-aware, for example by maintaining a continuous/unwrapped COG value or adding an angular-difference-aware PID input path; reset that state whenever the navigation PID is reset.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[555-561]
- src/main/common/fp_pid.c[60-73]
- src/main/navigation/navigation_pos_estimator.c[742-752]

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

---

Tip of the day

💡 Did you know, you can group findings by type and pick your Finding display, from Minimal to Full

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

[github-actions]

RAM / Flash usage vs. base branch — commit 2879562

No size baseline is available yet for this PR's base commit (no per-commit baseline has been published for it). This comment will show deltas once one exists — rebasing the PR refreshes its base commit.

Target	Flash Δ	RAM Δ
MATEKF405	704383 B (no baseline)	149520 B (no baseline)
MATEKF722	465539 B (no baseline)	125260 B (no baseline)
MATEKF765	736891 B (no baseline)	165432 B (no baseline)
MATEKH743	774007 B (no baseline)	168044 B (no baseline)

See RAM/flash optimization guide for techniques to reduce usage.

[github-actions]

Test firmware build ready — commit 2879562

Download firmware for PR #11831

247 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.