diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b012c040d27..a70d596c2cb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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()){ + // second choice is gps + currentspeed = GPS3DspeedFiltered; + *acc_ignore_slope_multipiler = 4.0f; + } else { //third choice is fixedWingReferenceAirspeed diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..713810dbfb8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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{ diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index b4b61f57669..dfc960f81b1 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -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); @@ -216,6 +217,7 @@ static void performPitotCalibrationCycle(void) } } + STATIC_PROTOTHREAD(pitotThread) { ptBegin(pitotThread); @@ -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 @@ -433,7 +436,7 @@ bool pitotHasFailed(void) return pitotHardwareFailed; } -bool pitotValidForAirspeed(void) +bool pitotValidateAirspeed(void) { bool ret = false; ret = pitotIsHealthy() && pitotIsCalibrationComplete(); @@ -492,4 +495,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 69451098ec8..e79920a81ac 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -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