From ac33e69fbe3b704023c3df89400a445eb8a756b1 Mon Sep 17 00:00:00 2001
From: Luca Erbetta <lucaerbetta105@gmail.com>
Date: Thu, 25 Apr 2019 20:06:07 +0200
Subject: [PATCH 1/4] Added aerospace functions

---
 sbs.conf                            |  2 +
 src/shared/utils/aero/AeroUtils.cpp | 25 ++++++++
 src/shared/utils/aero/AeroUtils.h   | 96 +++++++++++++++++++++++++++++
 3 files changed, 123 insertions(+)
 create mode 100644 src/shared/utils/aero/AeroUtils.cpp
 create mode 100644 src/shared/utils/aero/AeroUtils.h

diff --git a/sbs.conf b/sbs.conf
index 2c59d6456..31f5b7f64 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -106,6 +106,8 @@ Files:      src/shared/scheduler/TaskScheduler.cpp
             src/shared/math/Stats.cpp
             src/shared/math/Matrix.cpp
             src/shared/drivers/interrupt/external_interrupts.cpp
+            src/shared/utils/aero/AeroUtils.cpp
+
 [pwm]
 Type:       srcfiles
 Files:      src/shared/drivers/pwm/pwm.cpp
diff --git a/src/shared/utils/aero/AeroUtils.cpp b/src/shared/utils/aero/AeroUtils.cpp
new file mode 100644
index 000000000..c6c686c74
--- /dev/null
+++ b/src/shared/utils/aero/AeroUtils.cpp
@@ -0,0 +1,25 @@
+#include "AeroUtils.h"
+
+namespace aeroutils
+{
+
+float relAltitude(float pressure, float pressure_ref, float temperature_ref)
+{
+    using namespace constants;
+
+    return temperature_ref / a * (1 - powf(pressure / pressure_ref, n_inv));
+}
+
+float mslPressure(float pressure_ref, float temperature_ref, float altitude_ref)
+{
+    using namespace constants;
+    float T0 = mslTemperature(temperature_ref, altitude_ref);
+
+    return pressure_ref / powf(1 - a * altitude_ref / T0, n);
+}
+
+float mslTemperature(float temperature_ref, float altitude_ref)
+{
+    return temperature_ref - (altitude_ref * constants::a);
+}
+}  // namespace aeroutils
\ No newline at end of file
diff --git a/src/shared/utils/aero/AeroUtils.h b/src/shared/utils/aero/AeroUtils.h
new file mode 100644
index 000000000..819587ea8
--- /dev/null
+++ b/src/shared/utils/aero/AeroUtils.h
@@ -0,0 +1,96 @@
+/**
+ * Collection of functions for common aerospace calculations
+ *
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <cmath>
+
+namespace aeroutils
+{
+
+namespace constants
+{
+
+// Troposphere temperature gradient [deg/m]
+constexpr float a = 0.0065f;
+
+// Acceleration of gravity [m^s^2]
+constexpr float g = 9.80665f;
+
+// Air gas constant [J/(Kg*K])]
+constexpr float R = 287.05f;
+
+constexpr float n     = g / (R * a);
+constexpr float n_inv = (R * a) / g;
+
+}  // namespace constants
+
+/**
+ * Returns the current altitude with respect to a reference altitude for the
+ * given pressure, using International Standard Atmosphere  model.
+ *
+ * @warning This function is valid for altitudes below 11000 meters above sea
+ * level
+ * @warning This function provides a relative altitude from the reference
+ * altitude. It does not provide altitude above mean sea level unless the
+ * reference is, in fact, the sea level.
+ *
+ * @param pressure Current pressure [Pascal]
+ * @param pressure_ref Pressure at reference altitude [Pascal]
+ * @param temperature_ref Temperature at reference altitude [Kelvin]
+ * @return Current altitude with respect to the reference altitude [meters]
+ */
+float relAltitude(float pressure, float pressure_ref, float temperature_ref);
+
+/**
+ * Returns the expected pressure at mean sea level based on temperature and
+ * pressure at a reference altitude, using International Standard Atmosphere
+ * model.
+ *
+ * @warning This function is valid for altitudes below 11000 meters above sea
+ * level
+ *
+ * @param pressure_ref Pressure at reference altitude [Pascal]
+ * @param temperature_ref Temperature at reference altitude [Kelvin]
+ * @param altitude_ref Reference altitude [meters]
+ * @return Pressure at mean sea level [pascal]
+ */
+float mslPressure(float pressure_ref, float temperature_ref,
+                  float altitude_ref);
+
+/**
+ * Returns the expected temperature at mean sea level based on temperature at a
+ * reference altitude, using International Standard Atmosphere model.
+ *
+ * @warning This function is valid for altitudes below 11000 meters above sea
+ * level
+ *
+ * @param temperature_ref Temperature at reference altitude [Kelvin]
+ * @param altitude_ref Reference altitude [meters]
+ * @return Temperature at mean sea level [Kelvin]
+ */
+float mslTemperature(float temperature_ref, float altitude_ref);
+
+}  // namespace aeroutils
\ No newline at end of file
-- 
GitLab


From 979ef444812659d90754fb57d75466d91399afd0 Mon Sep 17 00:00:00 2001
From: Luca Erbetta <lucaerbetta105@gmail.com>
Date: Thu, 25 Apr 2019 20:07:10 +0200
Subject: [PATCH 2/4] [Piksi] Fix typo

---
 src/shared/drivers/piksi/piksi.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/shared/drivers/piksi/piksi.h b/src/shared/drivers/piksi/piksi.h
index 276b650ee..876a49e87 100644
--- a/src/shared/drivers/piksi/piksi.h
+++ b/src/shared/drivers/piksi/piksi.h
@@ -48,7 +48,7 @@ public:
 
     /**
      * \return the latest GPS data, or throws if the GPS has not yet got a fix.
-     * If the GPS has lost the fix, the same data is returened repeatedly,
+     * If the GPS has lost the fix, the same data is returned repeatedly,
      * use the timestamp field of the GPSData struct to know this.
      * \throws runtime_error is no data is available
      */
-- 
GitLab


From 148cf478616f3582eecf481bfaed95937844983e Mon Sep 17 00:00:00 2001
From: Luca Erbetta <lucaerbetta105@gmail.com>
Date: Mon, 29 Apr 2019 14:58:42 +0200
Subject: [PATCH 3/4] [AeroUtils] Wrong sign in mslTemperature function

---
 src/shared/utils/aero/AeroUtils.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/shared/utils/aero/AeroUtils.cpp b/src/shared/utils/aero/AeroUtils.cpp
index c6c686c74..2f1474a5d 100644
--- a/src/shared/utils/aero/AeroUtils.cpp
+++ b/src/shared/utils/aero/AeroUtils.cpp
@@ -20,6 +20,6 @@ float mslPressure(float pressure_ref, float temperature_ref, float altitude_ref)
 
 float mslTemperature(float temperature_ref, float altitude_ref)
 {
-    return temperature_ref - (altitude_ref * constants::a);
+    return temperature_ref + (altitude_ref * constants::a);
 }
 }  // namespace aeroutils
\ No newline at end of file
-- 
GitLab


From 22d709bfd2e10f60bbf070c33ae13c94f25b1f3a Mon Sep 17 00:00:00 2001
From: Luca Erbetta <lucaerbetta105@gmail.com>
Date: Mon, 29 Apr 2019 16:19:39 +0200
Subject: [PATCH 4/4] [AeroUtils] Added tests

---
 sbs.conf                            |  1 +
 src/shared/utils/aero/AeroUtils.cpp |  2 +-
 src/tests/catch/test-aero.cpp       | 81 +++++++++++++++++++++++++++++
 3 files changed, 83 insertions(+), 1 deletion(-)
 create mode 100644 src/tests/catch/test-aero.cpp

diff --git a/sbs.conf b/sbs.conf
index 31f5b7f64..471fb156a 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -152,6 +152,7 @@ Type:       srcfiles
 Files:      src/tests/catch/test-eventbroker.cpp
             src/tests/catch/test-circularbuffer.cpp
             src/tests/catch/test-xbee.cpp
+            src/tests/catch/test-aero.cpp
 
 
 #--------------------------#
diff --git a/src/shared/utils/aero/AeroUtils.cpp b/src/shared/utils/aero/AeroUtils.cpp
index 2f1474a5d..0b12a63ef 100644
--- a/src/shared/utils/aero/AeroUtils.cpp
+++ b/src/shared/utils/aero/AeroUtils.cpp
@@ -7,7 +7,7 @@ float relAltitude(float pressure, float pressure_ref, float temperature_ref)
 {
     using namespace constants;
 
-    return temperature_ref / a * (1 - powf(pressure / pressure_ref, n_inv));
+    return temperature_ref / a * (1 - powf(pressure / pressure_ref, n_inv)) ;
 }
 
 float mslPressure(float pressure_ref, float temperature_ref, float altitude_ref)
diff --git a/src/tests/catch/test-aero.cpp b/src/tests/catch/test-aero.cpp
new file mode 100644
index 000000000..c356242bb
--- /dev/null
+++ b/src/tests/catch/test-aero.cpp
@@ -0,0 +1,81 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * 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.
+ */
+
+#ifdef STANDALONE_CATCH1_TEST
+#include "catch-tests-entry.cpp"
+#endif
+
+#include <utils/catch.hpp>
+#include "utils/aero/AeroUtils.h"
+
+TEST_CASE("[AeroUtils] mslTemperature")
+{
+    using namespace aeroutils;
+    Approx isa_T0 =
+        Approx(288.151).margin(0.001);  // 15 deg celsius, 0.01% error allowed
+
+    // Test against various ISA altitudes
+    REQUIRE(mslTemperature(288.15, 0) == isa_T0);
+    REQUIRE(mslTemperature(287.825, 50) == isa_T0);
+    REQUIRE(mslTemperature(281.65, 1000) == isa_T0);
+    REQUIRE(mslTemperature(216.65, 11000) == isa_T0);
+    REQUIRE(mslTemperature(288.8, -100) == isa_T0);
+}
+
+TEST_CASE("[AeroUtils] mslPressure")
+{
+    using namespace aeroutils;
+    Approx isa_P0 =
+        Approx(101325).epsilon(0.0001);  // 15 deg celsius, 0.01% error allowed
+
+    // Test against various ISA altitudes
+    REQUIRE(mslPressure(101325, 288.15, 0) == isa_P0);
+    REQUIRE(mslPressure(100725.8, 287.825, 50) == isa_P0);
+    REQUIRE(mslPressure(89874.6, 281.65, 1000) == isa_P0);
+    REQUIRE(mslPressure(22632.1, 216.65, 11000) == isa_P0);
+    REQUIRE(mslPressure(102531.8, 288.8, -100) == isa_P0);
+}
+
+float mslAltitude(float pressure, float pressure_ref, float temperature_ref,
+                  float z_ref)
+{
+    using namespace aeroutils;
+    float t0 = mslTemperature(temperature_ref, z_ref);
+
+    return relAltitude(pressure,
+                       mslPressure(pressure_ref, temperature_ref, z_ref), t0);
+}
+
+TEST_CASE("[AeroUtils] relAltitude")
+{
+    REQUIRE(mslAltitude(101325, 101325, 288.150, 0) ==
+            Approx(0).epsilon(0.0001));
+    REQUIRE(mslAltitude(100726, 100726, 287.825, 50) ==
+            Approx(50).epsilon(0.0001));
+    REQUIRE(mslAltitude(89874.6, 89874.6, 281.650, 1000) ==
+            Approx(1000).epsilon(0.0001));
+    REQUIRE(mslAltitude(22632.1, 22632.1, 216.650, 11000) ==
+            Approx(11000).epsilon(0.0001));
+    REQUIRE(mslAltitude(102532, 102532, 288.800, -100) ==
+            Approx(-100).epsilon(0.0001));
+}
\ No newline at end of file
-- 
GitLab