diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 379a023ffd8..e5d2e3be75f 100644 --- a/src/main/flight/imu.c +++ b/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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 003ae97dded..576a1b70114 100644 --- a/src/main/flight/pid.c +++ b/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{ diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 3618923614a..6c6f81aafcc 100755 --- a/src/main/sensors/pitotmeter.c +++ b/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 */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index dc5ac422974..651059f4636 100755 --- a/src/main/sensors/pitotmeter.h +++ b/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