diff --git a/src/shared/utils/AeroUtils/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp
index 044f156556bd359f9ef6f2ab17d5c95f9148774a..a937551372be3dbd79ac7cfa9e849c04335be597 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.cpp
+++ b/src/shared/utils/AeroUtils/AeroUtils.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Luca Erbetta
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -83,6 +83,26 @@ Vector2f geodetic2NED(const Vector2f& gpsData, const Vector2f& offset)
};
}
+float computeRho(float d, float t0)
+{
+ float T = t0 + Constants::a * d;
+ return Constants::RHO_0 *
+ powf(T / Constants::MSL_TEMPERATURE,
+ Constants::g / (Constants::a * Constants::R) - 1.f);
+}
+
+float computeSoundSpeed(float d, float t0)
+{
+ float T = t0 + Constants::a * d;
+ float c = sqrt(Constants::GAMMA_AIR * Constants::R * T);
+ return c;
+}
+
+float computeMach(float d, float vtot, float t0)
+{
+ return vtot / computeSoundSpeed(d, t0);
+}
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
index be802b0c78a23a2eec349b4d685d0b8dc86031a8..ae170e3de0785fac4cb1734360e12531762cb983 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Luca Erbetta
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -180,6 +180,34 @@ float verticalSpeed(float p, float dpDt, float pRef, float tRef);
Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& position1,
const Eigen::Vector2f& position2);
+/**
+ * @brief Computes the rho (air density) of air at the given altitude.
+ *
+ * @param d Altitude agl in NED frame [m].
+ * @param t0 Temperature at ground level [K].
+ * @return The air density at the given altitude [kg/m^3].
+ */
+float computeRho(float d, float t0);
+
+/**
+ * @brief Computes the speed of sound at the given altitude.
+ *
+ * @param d Altitude agl in NED frame [m].
+ * @param t0 Temperature at ground level [K].
+ * @return The speed of sound at the given altitude [m/s].
+ */
+float computeSoundSpeed(float d, float t0);
+
+/**
+ * @brief Computes the mach relative to the speed at a certain altitude.
+ *
+ * @param d Altitude agl in NED frame [m].
+ * @param vtot Total speed [m/s].
+ * @param t0 Temperature at ground level [K].
+ * @return The mach relative to the speed at a certain altitude.
+ */
+float computeMach(float d, float vtot, float t0);
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index 566720d009e32d7a6f340f87238a4297befc4522..87481cb6a6df7fa7a73cbbdf89a22167fa6d01e3 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2015-2016 Skyward Experimental Rocketry
- * Author: Alain Carlucci
+/* Copyright (c) 2015-2023 Skyward Experimental Rocketry
+ * Authors: Alain Carlucci, Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -34,6 +34,7 @@ static constexpr float RADIANS_TO_DEGREES = 180.0f / PI;
static constexpr float g = 9.80665f; // [m^s^2]
+static constexpr float TROPOSPHERE_HEIGHT = 11000.f; // Troposphere height [m]
static constexpr float a = 0.0065f; // Troposphere temperature gradient [deg/m]
static constexpr float R = 287.05f; // Air gas constant [J/Kg/K]
static constexpr float n = g / (R * a);
@@ -42,7 +43,8 @@ static constexpr float nInv = (R * a) / g;
static constexpr float CO = 340.3; // Sound speed at ground altitude [m/s]
static constexpr float ALPHA = -3.871e-3; // Sound speed gradient [1/s]
static constexpr float RHO_0 = 1.225; // Air density at sea level [kg/m^3]
-static constexpr float Hn = 10400.0; // Scale height [m]
+static constexpr float GAMMA_AIR = 1.4f; // Adiabatic constant for air [1]
+static constexpr float Hn = 10400.0; // Scale height [m]
static constexpr float MSL_PRESSURE = 101325.0f; // [Pa]
static constexpr float MSL_TEMPERATURE = 288.15f; // [Kelvin]
diff --git a/src/tests/catch/test-aero.cpp b/src/tests/catch/test-aero.cpp
index fa1789c34bbb1442f44555bcbaf0f95012a8deb5..d1f52a663f8d6370c4a80f533fbb03ad47810910 100644
--- a/src/tests/catch/test-aero.cpp
+++ b/src/tests/catch/test-aero.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Luca Erbetta
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -115,3 +115,111 @@ TEST_CASE("[AeroUtils] verticalSpeed")
for (int i = 0; i < count; i++)
REQUIRE(verticalSpeed(p[i], dpDt[i], 100129.4, 297.5) == targetVSpeed);
}
+
+TEST_CASE("[AeroUtils] computeRho")
+{
+ {
+ // Test for same t0 of atmosisa
+ const float t0 = 288.15;
+ float d[] = {0, -500, -1000, -1500, -2000, -2500, -3000};
+
+ float rho[] = {1.2250, 1.1673, 1.1116, 1.0581, 1.0065, 0.9569, 0.9091};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeRho(d[i], t0) == Approx(rho[i]).epsilon(0.001));
+ }
+
+ {
+ // Test for a different t0 from atmosisa
+ const float t0 = 287.11;
+ float d[] = {0, -315.556, -631.111, -946.667, -1262.222,
+ -1577.778, -1893.333, -2208.889, -2524.444, -2840};
+
+ float rho[] = {1.2062937204963, 1.17004184985014, 1.13462949957852,
+ 1.10004288170165, 1.06626877484746, 1.03329364698175,
+ 1.0011045116218, 0.969688091422233, 0.939031634200062,
+ 0.909122116038058};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeRho(d[i], t0) == Approx(rho[i]).epsilon(0.001));
+ }
+}
+
+TEST_CASE("[AeroUtils] computeSoundSpeed")
+{
+ {
+ // Test for same t0 of atmosisa
+ const float t0 = 288.15;
+ float d[] = {0, -500, -1000, -1500, -2000, -2500, -3000};
+
+ float c[] = {340.2941, 338.3696, 336.4341, 334.4874,
+ 332.5293, 330.5596, 328.5781};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeSoundSpeed(d[i], t0) == Approx(c[i]).epsilon(0.001));
+ }
+
+ {
+ // Test for a different t0 from atmosisa
+ const float t0 = 287.11;
+ float d[] = {0, -315.556, -631.111, -946.667, -1262.222,
+ -1577.778, -1893.333, -2208.889, -2524.444, -2840};
+
+ float c[] = {339.679469143191, 338.463959192682, 337.244072148872,
+ 336.019752566034, 334.790959617651, 333.557636034153,
+ 332.319739232958, 331.077210026372, 329.830003980861,
+ 328.578059889884};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeSoundSpeed(d[i], t0) == Approx(c[i]).epsilon(0.001));
+ }
+}
+
+TEST_CASE("[AeroUtils] computeMach")
+{
+ {
+ // Test for same t0 of atmosisa
+ const float t0 = 288.15;
+ float d[] = {0, -500, -1000, -1500, -2000, -2500, -3000, -3000};
+ float vtot[] = {0, 100, 100, 100, 100, 100, 100, 0};
+
+ float mach[] = {0, 0.2955, 0.2972, 0.2990, 0.3007, 0.3025, 0.3043, 0};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeMach(d[i], vtot[i], t0) ==
+ Approx(mach[i]).epsilon(0.001));
+ }
+
+ {
+
+ // Test for a different t0 from atmosisa
+ const float t0 = 287.11;
+ float d[] = {0, -315.556, -631.111, -946.667, -1262.222,
+ -1577.778, -1893.333, -2208.889, -2524.444, -2840};
+ float vtot[] = {0,
+ 156.25199959656,
+ 312.503999193119,
+ 468.755998789678,
+ 625.007998386238,
+ 1250.01599677248,
+ 1031.26319733729,
+ 812.510397902109,
+ 593.757598466926,
+ 375.004799031743};
+
+ float mach[] = {0,
+ 0.461650333374514,
+ 0.926640451237251,
+ 1.39502512935622,
+ 1.86686044061653,
+ 3.74752624953993,
+ 3.10322582618053,
+ 2.45414173279215,
+ 1.80019280023227,
+ 1.14129591962841};
+
+ for (int i = 0; i < sizeof(d) / sizeof(float); i++)
+ REQUIRE(computeMach(d[i], vtot[i], t0) ==
+ Approx(mach[i]).epsilon(0.001));
+ }
+}
\ No newline at end of file