diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e060e558f89..cfa50a5f208 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..a187cf4c6e7 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -670,7 +670,9 @@ 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/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 00eeb71fe7f..7eedf6af8f1 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,9 +76,6 @@ 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); - PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); #define PITOT_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise @@ -152,7 +150,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; } @@ -210,11 +208,17 @@ bool pitotInit(void) 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 +237,6 @@ static void performPitotCalibrationCycle(void) } } - STATIC_PROTOTHREAD(pitotThread) { ptBegin(pitotThread); @@ -275,18 +278,21 @@ 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 + pitotAirspeedValidCached = pitotValidateAirspeed(); + + 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,65 +308,29 @@ 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 + ptYield(); } ptEnd(0); } -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; - } - } - return pitot.airSpeed; -} - -bool pitotIsHealthy(void) -{ - return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; -} - /** * Calculate virtual airspeed estimate (same as virtual pitot) * @@ -369,8 +339,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 +368,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 = pitotValidateAirspeed(); + 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 +436,22 @@ static float getVirtualAirspeedEstimate(void) static bool isPitotReadingPlausible(void) { #ifdef USE_GPS - if (!STATE(GPS_FIX)) { - return true; - } + if (STATE(GPS_FIX)) { + const float virtualAirspeedCmS = getVirtualAirspeedEstimate(); + const float minValidationSpeed = 700.0f; // 7 m/s - const float virtualAirspeedCmS = getVirtualAirspeedEstimate(); - const float minValidationSpeed = 700.0f; // 7 m/s + 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 - if (virtualAirspeedCmS < minValidationSpeed) { - return true; - } - - const float pitotAirspeedCmS = pitot.airSpeed; - - // 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; } /** @@ -482,12 +468,12 @@ bool pitotValidateAirspeed(void) { bool ret = false; ret = pitotIsHealthy() && pitotIsCalibrationComplete(); - +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) // For virtual pitot, we need GPS fix if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { - ret = ret && STATE(GPS_FIX); + return ret && STATE(GPS_FIX); } - +#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 && diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 94b15d29164..651059f4636 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -73,6 +73,5 @@ bool pitotIsHealthy(void); bool pitotValidateAirspeed(void); bool pitotGetValidForAirspeed(void); bool pitotHasFailed(void); -float getWindEstimatedVirtualAirspeed(void); #endif