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