Fix: Prioritize Airspeed over GPS for turn acceleration compensation

src/main/flight/imu.c

@@ -729,8 +729,17 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
     //fixed wing only
     static float lastspeed = -1.0f;
     float currentspeed = 0;
+#ifdef USE_PITOT
+    if (pitotGetValidForAirspeed())
+    {
+        // first choice is airspeed
+		currentspeed = getAirspeedEstimate();
+        *acc_ignore_slope_multipiler = 4.0f;
+    }
+    else
+#endif
     if (isGPSTrustworthy()) {
-        //first speed choice is gps
+        // second choice is gps
         static bool lastGPSHeartbeat;
         static pt1Filter_t GPS3DspeedFilter;
         static float GPS3DspeedFiltered = 0.0f;
@@ -742,13 +751,6 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
         currentspeed = GPS3DspeedFiltered;
         *acc_ignore_slope_multipiler = 4.0f;
     }
-#ifdef USE_PITOT
-    else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
-        // second choice is pitot
-		currentspeed = getAirspeedEstimate();
-        *acc_ignore_slope_multipiler = 2.0f;
-    }
-#endif
     else
     {
         //third choice is fixedWingReferenceAirspeed

src/main/flight/pid.c

@@ -554,7 +554,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{

src/main/sensors/pitotmeter.c

@@ -70,8 +70,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);
@@ -231,6 +232,7 @@ static void performPitotCalibrationCycle(void)
     }
 }
 
+
 STATIC_PROTOTHREAD(pitotThread)
 {
     ptBegin(pitotThread);
@@ -280,6 +282,7 @@ STATIC_PROTOTHREAD(pitotThread)
             pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
         }
 #endif
+        pitotAirspeedValidCached = pitotValidateAirspeed();
         ptYield();
 
         // Calculate IAS
@@ -449,7 +452,7 @@ bool pitotHasFailed(void)
     return pitotHardwareFailed;
 }
 
-bool pitotValidForAirspeed(void)
+bool pitotValidateAirspeed(void)
 {
     bool ret = false;
     ret = pitotIsHealthy() && pitotIsCalibrationComplete();
@@ -508,4 +511,9 @@ bool pitotValidForAirspeed(void)
 
     return ret;
 }
+
+bool pitotGetValidForAirspeed(void)
+{
+    return pitotAirspeedValidCached;
+}
 #endif /* PITOT */

src/main/sensors/pitotmeter.h

@@ -70,7 +70,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