diff --git a/src/shared/sensors/analog/Pitot/Pitot.h b/src/shared/sensors/analog/Pitot/Pitot.h
index 97e32249e0e0f14600d95fc87ef56bea4c87c8e2..952e3c2b90d87ec15e87dae7c4fe5882bc2c97bb 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 bd9f699214efad55a8ea2b20d5c95dd39847287d..1ec63ecabc80c730de76342e82940fb388c69370 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 e3e61661d46e90ee85f0de8b0a6b269683f316ba..96223d359399355a3a2f83621e647b9bfe0477c1 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