diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..67863411d66 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -172,7 +172,7 @@ main_sources(COMMON_SRC drivers/dronecan/libcanard/canard_stm32_driver.h drivers/dronecan/dronecan.c drivers/dronecan/dronecan.h - + drivers/display.c drivers/display.h drivers/display_canvas.c @@ -233,8 +233,6 @@ main_sources(COMMON_SRC drivers/pitotmeter/pitotmeter_dlvr_l10d.h drivers/pitotmeter/pitotmeter_msp.c drivers/pitotmeter/pitotmeter_msp.h - drivers/pitotmeter/pitotmeter_virtual.c - drivers/pitotmeter/pitotmeter_virtual.h drivers/pitotmeter/pitotmeter_fake.h drivers/pitotmeter/pitotmeter_fake.c drivers/pwm_esc_detect.c @@ -288,7 +286,7 @@ main_sources(COMMON_SRC drivers/vtx_common.c drivers/vtx_common.h - + fc/cli.c fc/cli.h fc/config.c diff --git a/src/main/drivers/pitotmeter/pitotmeter_virtual.c b/src/main/drivers/pitotmeter/pitotmeter_virtual.c deleted file mode 100644 index 368d663b604..00000000000 --- a/src/main/drivers/pitotmeter/pitotmeter_virtual.c +++ /dev/null @@ -1,94 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" - -#include "config/feature.h" - -#include "common/maths.h" -#include "common/utils.h" - -#include "fc/config.h" - -#include "flight/pid.h" -#include "flight/wind_estimator.h" - -#include "io/gps.h" - -#include "navigation/navigation.h" - -#include "sensors/pitotmeter.h" - -#include "drivers/pitotmeter/pitotmeter.h" -#include "drivers/pitotmeter/pitotmeter_virtual.h" - -#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) -static bool virtualPitotStart(pitotDev_t *pitot) -{ - UNUSED(pitot); - return true; -} - -static bool virtualPitotRead(pitotDev_t *pitot) -{ - UNUSED(pitot); - return true; -} - -static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature) -{ - UNUSED(pitot); - float airSpeed = 0.0f; - - if (pitotIsCalibrationComplete()) { -#if defined(USE_GPS) - if (isEstimatedWindSpeedValid() && STATE(GPS_FIX)) { - airSpeed = getWindEstimatedVirtualAirspeed(); - } - else if (STATE(GPS_FIX)) - { - airSpeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); - } - else -#endif - { - airSpeed = pidProfile()->fixedWingReferenceAirspeed; //float cm/s - } - } - - if (pressure) *pressure = sq(airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - if (temperature) *temperature = SSL_AIR_TEMPERATURE; // Temperature at standard sea level -} - -bool virtualPitotDetect(pitotDev_t *pitot) -{ - pitot->delay = 10000; - pitot->calibThreshold = 0.00001f; - pitot->start = virtualPitotStart; - pitot->get = virtualPitotRead; - pitot->calculate = virtualPitotCalculate; - return feature(FEATURE_GPS); -} -#endif diff --git a/src/main/drivers/pitotmeter/pitotmeter_virtual.h b/src/main/drivers/pitotmeter/pitotmeter_virtual.h deleted file mode 100644 index f59162dd028..00000000000 --- a/src/main/drivers/pitotmeter/pitotmeter_virtual.h +++ /dev/null @@ -1,20 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -bool virtualPitotDetect(pitotDev_t *pitot); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 108e48e2012..df8fe56d9fb 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -670,7 +670,7 @@ void init(void) #endif #ifdef USE_PITOT - pitotStartCalibration(); + if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) pitotStartCalibration(); #endif #if defined(USE_VTX_CONTROL) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 1db29a37330..a49806c3cb0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -924,7 +924,7 @@ void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTime uint16_t groundSpeedBoost = 0; #ifdef USE_PITOT - if (pitotValidateAirspeed()) { + if (pitotGetValidForAirspeed()) { static bool airspeedBoost = false; // Pitot available and airspeed source selected or low airspeed boost applied when using ground speed source if (!LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED) || airspeedBoost) { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index cf785827f04..ecab2efdc18 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -35,7 +35,6 @@ #include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h" #include "drivers/pitotmeter/pitotmeter_adc.h" #include "drivers/pitotmeter/pitotmeter_msp.h" -#include "drivers/pitotmeter/pitotmeter_virtual.h" #include "drivers/pitotmeter/pitotmeter_fake.h" #include "drivers/time.h" @@ -43,10 +42,12 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/imu.h" +#include "flight/pid.h" + #include "scheduler/protothreads.h" #include "sensors/barometer.h" -#include "flight/imu.h" #include "sensors/pitotmeter.h" #include "sensors/sensors.h" @@ -75,8 +76,7 @@ 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); +static bool isPitotAirspeedValid(void); PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); @@ -152,7 +152,7 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) case PITOT_VIRTUAL: if (pitotHardwareToUse != PITOT_AUTODETECT) { #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) - if (virtualPitotDetect(dev)) { + if (STATE(AIRPLANE) && feature(FEATURE_GPS)) { pitotHardware = PITOT_VIRTUAL; break; } @@ -207,14 +207,16 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) bool pitotInit(void) { - if (!pitotDetect(&pitot.dev, pitotmeterConfig()->pitot_hardware)) { - return false; - } + if (!pitotDetect(&pitot.dev, pitotmeterConfig()->pitot_hardware)) return false; + return true; } bool pitotIsCalibrationComplete(void) { +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) return true; +#endif return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration); } @@ -233,7 +235,6 @@ static void performPitotCalibrationCycle(void) } } - STATIC_PROTOTHREAD(pitotThread) { ptBegin(pitotThread); @@ -275,18 +276,19 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp); -#ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } -#endif + bool usePressureCalculation = true; #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { - pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; + pitot.airSpeed = fakePitotGetAirspeed(); + usePressureCalculation = false; + } +#endif +#ifdef USE_SIMULATOR + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + pitot.airSpeed = simulatorData.airSpeed; + usePressureCalculation = false; } #endif - pitotAirspeedValidCached = pitotValidateAirspeed(); - ptYield(); // Calculate IAS if (pitotIsCalibrationComplete()) { @@ -302,63 +304,30 @@ STATIC_PROTOTHREAD(pitotThread) // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!! currentTimeUs = micros(); + if (usePressureCalculation) { + if (pitotmeterConfig()->pitot_lpf_milli_hz) { + pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); + } else { + pitot.pressure = pitotPressureTmp; + } - if (pitotmeterConfig()->pitot_lpf_milli_hz) { - pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); - } else { - pitot.pressure = pitotPressureTmp; + pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; // cm/s + pitot.temperature = pitotTemperatureTmp; // Kelvin } pitot.lastMeasurementUs = currentTimeUs; - - pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; // cm/s - pitot.temperature = pitotTemperatureTmp; // Kelvin - } else { pitot.pressure = pitotPressureTmp; performPitotCalibrationCycle(); pitot.airSpeed = 0.0f; } -#if defined(USE_PITOT_FAKE) - if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { - pitot.airSpeed = fakePitotGetAirspeed(); - } -#endif -#ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; - } -#endif - } - - ptEnd(0); -} + // Check pitot airspeed validity and cache result for use by external functions + pitotAirspeedValidCached = isPitotAirspeedValid(); -void pitotUpdate(void) -{ - pitotThread(); -} - -/* - * Airspeed estimate in cm/s - * Returns hardware pitot if valid, GPS-based virtual airspeed if pitot failed, - * or raw pitot value as last resort - */ -float getAirspeedEstimate(void) -{ - // If hardware pitot has failed validation, use GPS-based virtual airspeed - if (pitotHardwareFailed) { - float virtualAirspeed = getVirtualAirspeedEstimate(); - if (virtualAirspeed > 0.0f) { - return virtualAirspeed; - } + ptYield(); } - return pitot.airSpeed; -} -bool pitotIsHealthy(void) -{ - return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; + ptEnd(0); } /** @@ -369,8 +338,9 @@ bool pitotIsHealthy(void) * * @return virtual airspeed in cm/s, or 0 if GPS unavailable */ + #if defined(USE_GPS) && defined(USE_WIND_ESTIMATOR) -float getWindEstimatedVirtualAirspeed(void) +static float getWindEstimatedVirtualAirspeed(void) { static float virtualAirspeed = 0.0f; static timeMs_t lastUpdateTimeMs = 0; @@ -397,31 +367,55 @@ float getWindEstimatedVirtualAirspeed(void) #endif static float getVirtualAirspeedEstimate(void) { -#if defined(USE_GPS) && defined(USE_WIND_ESTIMATOR) - if (!STATE(GPS_FIX)) { - return 0.0f; + pitot.lastSeenHealthyMs = millis(); +#if defined(USE_GPS) + if (STATE(GPS_FIX)) { +#if defined(USE_WIND_ESTIMATOR) + if (isEstimatedWindSpeedValid()) { + return getWindEstimatedVirtualAirspeed(); + } else +#endif + { + return posControl.actualState.vel3D; + } } +#endif + return pidProfile()->fixedWingReferenceAirspeed; //float cm/s +} - float airSpeed = 0.0f; - - // Use wind estimator if available (matches virtual pitot logic) - if (isEstimatedWindSpeedValid()) { - airSpeed = getWindEstimatedVirtualAirspeed(); - } else { - // Fall back to raw GPS velocity if no wind estimator - airSpeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); +void pitotUpdate(void) +{ +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + pitot.airSpeed = getVirtualAirspeedEstimate(); + pitotAirspeedValidCached = isPitotAirspeedValid(); + return; } +#endif + pitotThread(); +} - return airSpeed; -#elif defined(USE_GPS) - // No wind estimator, use raw GPS velocity - if (!STATE(GPS_FIX)) { - return 0.0f; +/* + * Airspeed estimate in cm/s + * Returns hardware pitot if valid, GPS-based virtual airspeed if pitot failed, + * or raw pitot value as last resort + */ +float getAirspeedEstimate(void) +{ + // If hardware pitot has failed validation, use GPS-based virtual airspeed + if (pitotHardwareFailed) { + float virtualAirspeed = getVirtualAirspeedEstimate(); + if (virtualAirspeed > 0.0f) { + return virtualAirspeed; + } } - return calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); -#else - return 0.0f; -#endif + + return pitot.airSpeed; +} + +bool pitotIsHealthy(void) +{ + return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; } /** @@ -441,31 +435,22 @@ static float getVirtualAirspeedEstimate(void) static bool isPitotReadingPlausible(void) { #ifdef USE_GPS - if (!STATE(GPS_FIX)) { - return true; - } - - const float virtualAirspeedCmS = getVirtualAirspeedEstimate(); - const float minValidationSpeed = 700.0f; // 7 m/s - - if (virtualAirspeedCmS < minValidationSpeed) { - return true; - } + if (STATE(GPS_FIX)) { + const float virtualAirspeedCmS = getVirtualAirspeedEstimate(); + const float minValidationSpeed = 700.0f; // 7 m/s - const float pitotAirspeedCmS = pitot.airSpeed; + if (virtualAirspeedCmS > minValidationSpeed) { + // Wide thresholds to catch gross failures (blocked pitot) only + const float minPlausibleAirspeed = virtualAirspeedCmS * 0.3f; // 30% of virtual + const float maxPlausibleAirspeed = virtualAirspeedCmS * 2.0f; // 200% of virtual - // Wide thresholds to catch gross failures (blocked pitot) only - const float minPlausibleAirspeed = virtualAirspeedCmS * 0.3f; // 30% of virtual - const float maxPlausibleAirspeed = virtualAirspeedCmS * 2.0f; // 200% of virtual - - if (pitotAirspeedCmS < minPlausibleAirspeed || pitotAirspeedCmS > maxPlausibleAirspeed) { - return false; + if (pitot.airSpeed < minPlausibleAirspeed || pitot.airSpeed > maxPlausibleAirspeed) { + return false; + } + } } - - return true; -#else - return true; #endif + return true; } /** @@ -478,21 +463,19 @@ bool pitotHasFailed(void) return pitotHardwareFailed; } -bool pitotValidateAirspeed(void) +static bool isPitotAirspeedValid(void) { bool ret = false; ret = pitotIsHealthy() && pitotIsCalibrationComplete(); - - // For virtual pitot, we need GPS fix +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + // For virtual pitot, we need GPS fix and valid wind estimate if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { - ret = ret && STATE(GPS_FIX) && isEstimatedWindSpeedValid(); + return ret && STATE(GPS_FIX) && isEstimatedWindSpeedValid(); } - +#endif // For hardware pitot sensors, validate readings against GPS when armed // This detects blocked or failed pitot tubes - if (ret && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL && - detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { - + if (ret && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (ARMING_FLAG(ARMED)) { // Check if pitot reading is plausible if (!isPitotReadingPlausible()) { diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 94b15d29164..21a823329c1 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -70,9 +70,7 @@ void pitotStartCalibration(void); void pitotUpdate(void); float getAirspeedEstimate(void); bool pitotIsHealthy(void); -bool pitotValidateAirspeed(void); bool pitotGetValidForAirspeed(void); bool pitotHasFailed(void); -float getWindEstimatedVirtualAirspeed(void); #endif