From 4d134d250393ebaf06a4193dbe98ef95333c3a1d Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Thu, 22 Aug 2024 16:16:57 +0200
Subject: [PATCH] [Pitot] Extracted code into AeroUtils
---
src/shared/sensors/analog/Pitot/Pitot.h | 13 +++++--------
src/shared/utils/AeroUtils/AeroUtils.cpp | 18 +++++++++++++++++-
src/shared/utils/AeroUtils/AeroUtils.h | 22 ++++++++++++++++++++++
3 files changed, 44 insertions(+), 9 deletions(-)
diff --git a/src/shared/sensors/analog/Pitot/Pitot.h b/src/shared/sensors/analog/Pitot/Pitot.h
index 97e32249e..952e3c2b9 100644
--- a/src/shared/sensors/analog/Pitot/Pitot.h
+++ b/src/shared/sensors/analog/Pitot/Pitot.h
@@ -64,14 +64,11 @@ protected:
if (totalPressure > 0 && staticPressure > 0 &&
totalPressure > staticPressure)
{
-
- float gamma = 1.4f;
- float c = sqrt(gamma * Constants::R * reference.refTemperature);
- // clang-format off
- float M = sqrt(((pow(totalPressure / staticPressure, (gamma - 1) / gamma)) - 1) * (2 / (gamma - 1)));
- // clang-format on
- pitotSpeed.airspeed = M * c;
- pitotSpeed.deltaP = totalPressure - staticPressure;
+ // NOTE: Here we assume that we are always at refTemperature, so
+ // calculations might be wrong at higher elevations!
+ pitotSpeed.airspeed = Aeroutils::computePitotAirspeed(
+ totalPressure, staticPressure, 0, reference.refTemperature);
+ pitotSpeed.deltaP = totalPressure - staticPressure;
}
else
{
diff --git a/src/shared/utils/AeroUtils/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp
index bd9f69921..1ec63ecab 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.cpp
+++ b/src/shared/utils/AeroUtils/AeroUtils.cpp
@@ -94,7 +94,7 @@ float computeRho(float d, float t0)
float computeSoundSpeed(float d, float t0)
{
float T = t0 + Constants::a * d;
- float c = sqrt(Constants::GAMMA_AIR * Constants::R * T);
+ float c = sqrtf(Constants::GAMMA_AIR * Constants::R * T);
return c;
}
@@ -103,6 +103,22 @@ float computeMach(float d, float vtot, float t0)
return vtot / computeSoundSpeed(d, t0);
}
+float computePitotMach(float pressureTotal, float pressureStatic)
+{
+ return sqrtf(((powf(pressureTotal / pressureStatic,
+ (Constants::GAMMA_AIR - 1) / Constants::GAMMA_AIR)) -
+ 1) *
+ (2 / (Constants::GAMMA_AIR - 1)));
+}
+
+float computePitotAirspeed(float pressureTotal, float pressureStatic, float d,
+ float t0)
+{
+ float c = computeSoundSpeed(d, t0);
+ float M = computePitotMach(pressureTotal, pressureStatic);
+ return M * c;
+}
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
index e3e61661d..96223d359 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -209,6 +209,28 @@ float computeSoundSpeed(float d, float t0);
*/
float computeMach(float d, float vtot, float t0);
+/**
+ * @brief Computes the mach from total and static pressure measures from a pitot
+ * tube.
+ *
+ * @param pressureTotal Total pressure from the pitot tube [Pa].
+ * @param pressureStatic Static pressure from the pitot tube [Pa].
+ * @return Calculated mach.
+ */
+float computePitotMach(float pressureTotal, float pressureStatic);
+
+/**
+ * @brief Computes air speed relative to the pitot tube
+ *
+ * @param pressureTotal Total pressure from the pitot tube [Pa].
+ * @param pressureStatic Static pressure from the pitot tube [Pa].
+ * @param d Altitude agl in NED frame [m].
+ * @param t0 Temperature at ground level [K].
+ * @return Calculated airspeed [m/s].
+ */
+float computePitotAirspeed(float pressureTotal, float pressureStatic, float d,
+ float t0);
+
} // namespace Aeroutils
} // namespace Boardcore
--
GitLab