[Pitot] Extracted code into AeroUtils

src/shared/sensors/analog/Pitot/Pitot.h

@@ -64,13 +64,10 @@ 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;
+            // 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

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

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