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