Position estimator sensor correction refactor#11322
Position estimator sensor correction refactor#11322breadoven wants to merge 6 commits intoiNavFlight:maintenance-9.xfrom
Conversation
Review Summary by QodoRefactor sensor corrections to apply only on sensor updates
WalkthroughsDescription• Apply sensor corrections only once per sensor update, not every loop iteration • Change EPH/EPV degradation from exponential to linear with 10s timeout • Replace velocity decay correction with direct velocity reduction • Track sensor update time deltas in GPS, Baro, and Flow structures • Constrain corrections based on actual sensor update intervals Diagramflowchart LR
A["Sensor Update<br/>GPS/Baro/Flow"] -->|Store updateDt| B["Sensor Data<br/>Structure"]
B -->|Check updateDt| C{"Sensor<br/>Updated?"}
C -->|Yes| D["Calculate<br/>Corrections"]
C -->|No| E["Skip<br/>Corrections"]
D --> F["Apply Once<br/>Per Update"]
E --> G["Degrade EPH/EPV<br/>Linearly"]
F --> H["Reset updateDt<br/>to Zero"]
G --> I["Estimate Position"]
File Changes1. src/main/navigation/navigation_pos_estimator.c
|
Code Review by Qodo
✅ 1.
|
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
| #define INAV_EST_CORR_LIMIT_VALUE 4000.0f | ||
|
|
||
| #define INAV_EST_CORR_LIMIT_VALUE 4000.0f // Sanity constraint limit for pos/vel estimate correction value (max 40m correction per s) |
There was a problem hiding this comment.
2. Duplicate macro define 🐞 Bug ✓ Correctness
INAV_EST_CORR_LIMIT_VALUE is defined twice in the same header, causing a macro redefinition warning and potentially a build failure when warnings are treated as errors.
Agent Prompt
### Issue description
`INAV_EST_CORR_LIMIT_VALUE` is defined twice in `navigation_pos_estimator_private.h`, which can break builds when warnings are treated as errors.
### Issue Context
The PR adds a second `#define INAV_EST_CORR_LIMIT_VALUE 4000.0f` without removing the existing one.
### Fix Focus Areas
- src/main/navigation/navigation_pos_estimator_private.h[35-39]
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
|
@breadoven Out of interest before testing. How many of these changes cross over with changes you made in the other one merged two days back.. I asked because I wasn't sure how that last one sat with respect to the master. And if the action builds for these commits will also include the merge of the last one ? |
The Action build for this PR includes the changes from the other PR. They're both against the maintenance-9.x branch. That's the reason I merged the other one so it was included in this PR otherwise things get a bit messy given they interact with each other. I'm not sure if this PR will be in 9.1 though, needs some decent testing given the extent of the changes. |
|
I did a quick test of this just carrying an armed quad with a GPS lock. It seems to work as expected except for odd behaviour of the GPS updates. The debugs I was using are in the Edit: Checking the last HITL log the 5s / 0.5s glitch is absent. |
|
Tested it with another 3" quad today that always has rather average satellite acquisition performance... When I took-off the HDOP was 1.8. When I first enabled Poshold it drifted a little on the XYZ, by about 0.5m from what the log showed and from what I remember LOS.. But then it settled down on the XY within a few seconds. And still continued to drift up and down by about 1m. Which I didn't think was too bad considering the strong wind blowing over the foam covered baro.. But as soon as I switched out of Poshold back to Angle for a few seconds and then back to Poshold, It sat very firmly positioned on all axis's for the rest of the flight. I only had a quick look at what you changed. And one thing than now noticeably works, is the altitude throttle stick adjustment in Poshold. Looking at the log. The This quad was never anything special as far as navigation performance. So it's refreshing to see it work this way. |
|
More debugging and it appears the 5s glitch is caused by Position Estimator Edit: |
|
Finally the 5s glitch has been tracked down. It's caused by It's not immediately obvious what this is doing but I assume it's behaving as intended ... or maybe not ? |
|
Nice work, Breadoven! I believe it's checking which constellations are currently enabled, etc. With a 500ms timeout. It is intended to still process the position data in the loop -- if the queries are answered in a timely fashion. I will make a PR fixing it in a moment. |
3 participants
PR changes the way sensor corrections are applied in position estimator. Currently corrections are applied every loop which doesn't make sense given sensors update at a much slower rate than loop rate. This leads to corrections repeatedly being applied that are correcting back to an out of date sensor position which isn't be beneficial and wastes processor time.
This PR changes the corrections so they are only applied once immediately after a sensor update occurs. It affects the GPS, Baro and Flow sensors that are used for position estimator correction input. In between sensor updates the estimation will rely on the accelerometer based input only.
Only other related changes are:
posEstimator.est.vel(fits better with the new corrections logic and does the same thing ultimately)Tested OK on HITL so far which only covers the GPS and Baro. The Flow sensor changes need further testing.