Position estimator corrections improvement and estimated velocity filtering#11387
Open
breadoven wants to merge 3 commits intoiNavFlight:maintenance-9.xfrom
Open
Position estimator corrections improvement and estimated velocity filtering#11387breadoven wants to merge 3 commits intoiNavFlight:maintenance-9.xfrom
breadoven wants to merge 3 commits intoiNavFlight:maintenance-9.xfrom
Conversation
Contributor
Review Summary by QodoPosition estimator corrections separation and velocity filtering
WalkthroughsDescription• Split XY and Z position estimator corrections into separate flags - Allows independent updates when XY or Z sensor data arrives • Improved EPH/EPV degradation logic with 200ms timeout threshold - Only degrades when no sensor updates occur for >200ms • Applied PT1 filtering to velocity estimates - Reduces noise in XY and Z velocity publications • Optimized time macro for frequency-to-seconds conversion Diagramflowchart LR
A["Sensor Updates<br/>XY & Z"] --> B["Split Correction<br/>Flags"]
B --> C["Independent<br/>XY Corrections"]
B --> D["Independent<br/>Z Corrections"]
C --> E["Apply Corrections<br/>Per Axis"]
D --> E
E --> F["PT1 Filter<br/>Velocity"]
F --> G["Publish Filtered<br/>Velocity"]
H["200ms Timeout<br/>Logic"] --> I["Degrade EPH/EPV<br/>Only on Timeout"]
I --> J["Update Estimates"]
File Changes1. src/main/common/time.h
|
Contributor
Code Review by Qodo
1. Float US2MS() in timeouts
|
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
Comment on lines
+764
to
+767
| const bool XYSensorUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200; | ||
| ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); | ||
| const bool ZSensorUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200; | ||
| ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); |
Contributor
There was a problem hiding this comment.
1. Float us2ms() in timeouts 📘 Rule violation ✓ Correctness
The new sensor-update timeout checks mix float millisecond math (US2MS) with integer timeMs_t, which can lose precision and yield incorrect timeout behavior after longer uptimes. This violates the requirement to keep consistent units/types and avoid precision-losing conversions in math-heavy code.
Agent Prompt
## Issue description
The new XY/Z sensor timeout checks use `US2MS(currentTimeUs)` which returns a float, then subtract `timeMs_t` integer timestamps. This mixes float and integer time units and can lose precision or behave unexpectedly over long uptimes.
## Issue Context
`timeMs_t` is `uint32_t`, while `US2MS()` is a float conversion macro. Timeout checks should be done using consistent integer time units.
## Fix Focus Areas
- src/main/navigation/navigation_pos_estimator.c[764-767]
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
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
1 participant
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.
PR splits out handling of XY and Z position estimator corrections so the corrections are updated separately when new XY or Z sensor data is available.
Estimated EPH and EPV degradation logic also changed so EPH/EPV are only degraded if no sensor updates occur for >200ms rather than always degrading them between normal sensor update intervals.
Filtering has also been applied to the velocity estimates to remove some of the noise that seems to affect velocity in particular.
HITL tested and also on a quad without issues.