diff --git a/CMakeLists.txt b/CMakeLists.txt
index d86b09e409cf573e99173fa6d3034aa0d87089f0..bfc731dc5245ac56598a9207597847d7f0914f3e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -133,6 +133,7 @@ add_executable(catch-tests-boardcore
src/tests/catch/test-modulemanager.cpp
src/tests/catch/test-MEA.cpp
src/tests/catch/test-airbrakesInterp.cpp
+ src/tests/catch/test-pitot.cpp
)
target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS)
sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery)
diff --git a/src/shared/sensors/analog/Pitot/Pitot.h b/src/shared/sensors/analog/Pitot/Pitot.h
index cbc63104e024e75f1398476f07a83e0eee24e6ee..b598e3a936d1aff65a745edde992483c3dffbfec 100644
--- a/src/shared/sensors/analog/Pitot/Pitot.h
+++ b/src/shared/sensors/analog/Pitot/Pitot.h
@@ -23,6 +23,7 @@
#pragma once
#include <algorithms/ReferenceValues.h>
+#include <drivers/timer/TimestampTimer.h>
#include <sensors/Sensor.h>
#include <utils/AeroUtils/AeroUtils.h>
@@ -36,9 +37,9 @@ namespace Boardcore
class Pitot : public Sensor<PitotData>
{
public:
- Pitot(std::function<PressureData()> getPitotPressure,
+ Pitot(std::function<float()> getTotalPressure,
std::function<float()> getStaticPressure)
- : getPitotPressure(getPitotPressure),
+ : getTotalPressure(getTotalPressure),
getStaticPressure(getStaticPressure)
{
}
@@ -56,27 +57,24 @@ public:
PitotData sampleImpl() override
{
- float airDensity = Aeroutils::relDensity(
- getStaticPressure(), reference.refPressure, reference.refAltitude,
- reference.refTemperature);
+ float totalPressure = getTotalPressure();
+ float staticPressure = getStaticPressure();
- if (airDensity != 0.0)
- {
- PitotData pitotSpeed;
+ // clang-format off
+ float gamma = 1.4f;
+ float c = sqrt(gamma * Constants::R * reference.refTemperature);
+ float M = sqrt(((pow(totalPressure / staticPressure, (gamma - 1) / gamma)) - 1) * (2 / (gamma - 1)));
+ // clang-format on
- pitotSpeed.timestamp = getPitotPressure().pressureTimestamp;
- pitotSpeed.deltaP = getPitotPressure().pressure;
- pitotSpeed.airspeed =
- sqrtf(2 * fabs(pitotSpeed.deltaP) / airDensity);
+ PitotData pitotSpeed;
+ pitotSpeed.airspeed = M * c;
+ pitotSpeed.timestamp = TimestampTimer::getTimestamp();
- return pitotSpeed;
- }
-
- return lastSample;
+ return pitotSpeed;
}
private:
- std::function<PressureData()> getPitotPressure;
+ std::function<float()> getTotalPressure;
std::function<float()> getStaticPressure;
ReferenceValues reference;
diff --git a/src/tests/catch/test-pitot.cpp b/src/tests/catch/test-pitot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6fef5533f33dc84d6e7b3b95daa8baae51992e60
--- /dev/null
+++ b/src/tests/catch/test-pitot.cpp
@@ -0,0 +1,73 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <sensors/analog/Pitot/Pitot.h>
+
+#include <catch2/catch.hpp>
+#include <functional>
+#include <iostream>
+
+using namespace Boardcore;
+
+float totalPressures[] = {142319.922179851, 142334.556779026, 142334.990339878,
+ 142328.593255716, 142334.485137298, 142355.994981968,
+ 142375.515131591, 142371.90326022, 142369.472114238,
+ 142372.026452516, 142374.830116745};
+float staticPressures[] = {89427.1259035819, 89399.4135799592, 89370.6155267334,
+ 89337.8306466511, 89307.6055280662, 89287.1092122239,
+ 89265.7025543447, 89231.9778726411, 89199.4339163267,
+ 89171.8754442722, 89144.5662981688};
+float results[] = {291.616036301297, 291.754323018779, 291.863248891676,
+ 291.971052285523, 292.098174588202, 292.225543982517,
+ 292.351618537494, 292.469497902937, 292.585736754635,
+ 292.695003991533, 292.803923124566};
+constexpr float referenceTemperature = 298.15;
+constexpr unsigned int SAMPLES_N = 10;
+
+TEST_CASE("Pitot Test")
+{
+ Pitot pitot(
+ []()
+ {
+ static int i = 0;
+ return totalPressures[i++];
+ },
+ []()
+ {
+ static int i = 0;
+ return staticPressures[i++];
+ });
+ ReferenceValues references;
+ references.refTemperature = referenceTemperature;
+
+ pitot.setReferenceValues(references);
+
+ for (int i = 0; i < SAMPLES_N; i++)
+ {
+ pitot.sample();
+ if (pitot.getLastSample().airspeed != Approx(results[i]).epsilon(0.01))
+ {
+ FAIL("The computed value differs from the correct one: "
+ << pitot.getLastSample().airspeed << " != " << results[i]);
+ }
+ }
+}
\ No newline at end of file