Skip to content

Fix: Prioritize Airspeed over GPS for turn acceleration compensation #11312

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
shota3527 wants to merge 3 commits into
base: maintenance-9.x
Choose a base branch
from
Open

Fix: Prioritize Airspeed over GPS for turn acceleration compensation #11312

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8a5b8ea
Refactor airspeed selection logic in imuCalculateTurnRateacceleration…
shota3527 Feb 5, 2026
b7dc9f1
Rename pitotValidForAirspeed to pitotGetValidForAirspeed and update r…
shota3527 Feb 6, 2026
81492f9
Update comments for airspeed selection logic in imuCalculateTurnRatea…
shota3527 Feb 6, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 9 additions & 8 deletions src/main/flight/imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -703,19 +703,20 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
//fixed wing only
static float lastspeed = -1.0f;
float currentspeed = 0;
if (isGPSTrustworthy()){
//first speed choice is gps
currentspeed = GPS3DspeedFiltered;
*acc_ignore_slope_multipiler = 4.0f;
}
#ifdef USE_PITOT
else if (sensors(SENSOR_PITOT) && pitotIsHealthy())
if (pitotGetValidForAirspeed())
{
// second choice is pitot
// first choice is airspeed
currentspeed = getAirspeedEstimate();
*acc_ignore_slope_multipiler = 2.0f;
*acc_ignore_slope_multipiler = 4.0f;
}
else
#endif
if (isGPSTrustworthy()){
Comment on lines 706 to +715
Copy link
Copy Markdown
Contributor

@qodo-code-review qodo-code-review Bot Feb 5, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

1. Pitot failure trips early 🐞 Bug ⛯ Reliability

• imuCalculateTurnRateacceleration() now calls pitotValidForAirspeed() from the main PID/attitude
  loop hot-path.
• pitotValidForAirspeed() increments pitotFailureCounter per call and uses PITOT_FAILURE_THRESHOLD
  with an explicit ~100Hz timing assumption; at PID-loop rates this can declare pitot failure orders
  of magnitude faster than intended.
• Result can be false pitot failure detection (forcing GPS/virtual fallback) and additional CPU load
  in a critical control loop.
Agent Prompt 
### Issue description
`imuCalculateTurnRateacceleration()` calls `pitotValidForAirspeed()` in the main PID/attitude loop. `pitotValidForAirspeed()` updates failure counters per call and uses thresholds documented for ~100Hz operation, so calling it at PID-loop rates can cause premature pitot failure detection and unnecessary CPU load.

### Issue Context
- `pitotValidForAirspeed()` contains stateful failure detection (`pitotFailureCounter`, `pitotHardwareFailed`) and assumes a bounded call frequency.
- IMU/attitude is updated from `taskMainPidLoop`, typically much faster than PITOT task frequency.

### Fix Focus Areas
- src/main/flight/imu.c[701-719]
- src/main/sensors/pitotmeter.c[436-493]
- src/main/fc/fc_tasks.c[571-577]
- src/main/fc/fc_core.c[904-939]

### Suggested approach
- Add a cached boolean (e.g., `pitotAirspeedValidCached`) updated **once per PITOT task run** (or in `pitotUpdate()`), by calling `pitotValidForAirspeed()` there.
- In `imuCalculateTurnRateacceleration()`, use the cached boolean instead of calling `pitotValidForAirspeed()` directly.
- If needed, expose a side-effect-free accessor for IMU usage (e.g., `pitotIsHealthy() && pitotIsCalibrationComplete() && !pitotHasFailed()`) and keep plausibility counters in the PITOT update task only.

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

// second choice is gps
currentspeed = GPS3DspeedFiltered;
*acc_ignore_slope_multipiler = 4.0f;
}
else
{
//third choice is fixedWingReferenceAirspeed
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -552,7 +552,7 @@ void updatePIDCoefficients(void)
float tpaFactor=1.0f;
float iTermFactor=1.0f; // Separate factor for I-term scaling
if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation
if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){
if(currentControlProfile->throttle.apa_pow>0 && pitotGetValidForAirspeed()){
tpaFactor = calculateFixedWingAirspeedTPAFactor();
iTermFactor = calculateFixedWingAirspeedITermFactor(); // Less aggressive I-term scaling
}else{
Expand Down
16 changes: 12 additions & 4 deletions src/main/sensors/pitotmeter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,9 @@ pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
static bool pitotHardwareFailed = false;
static uint16_t pitotFailureCounter = 0;
static uint16_t pitotRecoveryCounter = 0;
#define PITOT_FAILURE_THRESHOLD 20 // 0.2 seconds at 100Hz - fast detection per LOG00002 analysis
#define PITOT_RECOVERY_THRESHOLD 200 // 2 seconds of consecutive good readings to recover
static bool pitotAirspeedValidCached = false;
#define PITOT_FAILURE_THRESHOLD 10 // 0.2 seconds at 50Hz - fast detection per LOG00002 analysis
#define PITOT_RECOVERY_THRESHOLD 100 // 2 seconds of consecutive good readings to recover

// Forward declaration for GPS-based airspeed fallback
static float getVirtualAirspeedEstimate(void);
Expand Down Expand Up @@ -216,6 +217,7 @@ static void performPitotCalibrationCycle(void)
}
}


STATIC_PROTOTHREAD(pitotThread)
{
ptBegin(pitotThread);
Expand Down Expand Up @@ -263,7 +265,8 @@ STATIC_PROTOTHREAD(pitotThread)
if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
}
#endif
#endif
pitotAirspeedValidCached = pitotValidateAirspeed();
ptYield();

// Calculate IAS
Expand Down Expand Up @@ -433,7 +436,7 @@ bool pitotHasFailed(void)
return pitotHardwareFailed;
}

bool pitotValidForAirspeed(void)
bool pitotValidateAirspeed(void)
{
bool ret = false;
ret = pitotIsHealthy() && pitotIsCalibrationComplete();
Expand Down Expand Up @@ -492,4 +495,9 @@ bool pitotValidForAirspeed(void)

return ret;
}

bool pitotGetValidForAirspeed(void)
{
return pitotAirspeedValidCached;
}
#endif /* PITOT */
3 changes: 2 additions & 1 deletion src/main/sensors/pitotmeter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,8 @@ void pitotStartCalibration(void);
void pitotUpdate(void);
float getAirspeedEstimate(void);
bool pitotIsHealthy(void);
bool pitotValidForAirspeed(void);
bool pitotValidateAirspeed(void);
bool pitotGetValidForAirspeed(void);
bool pitotHasFailed(void);

#endif