From 0fec810d2ab44644b26878d56a0d7f23f24291f4 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sun, 10 Jul 2022 11:25:36 +0000
Subject: [PATCH] [ReferenceValues] Added gps start position

---
 src/shared/algorithms/ReferenceValues.h | 16 ++++++++++++++--
 src/shared/utils/Constants.h            | 15 +++++++++------
 2 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/src/shared/algorithms/ReferenceValues.h b/src/shared/algorithms/ReferenceValues.h
index d984dc9fa..fcd4de5df 100644
--- a/src/shared/algorithms/ReferenceValues.h
+++ b/src/shared/algorithms/ReferenceValues.h
@@ -24,6 +24,7 @@
 
 #include <utils/Constants.h>
 
+#include <Eigen/Eigen>
 #include <ostream>
 
 namespace Boardcore
@@ -39,6 +40,10 @@ struct ReferenceValues
     float pressure;
     float temperature;
 
+    // Start position
+    float startLatitude;
+    float startLongitude;
+
     // Pressure and temperature at mean sea level for altitude calculation
     float mslPressure    = Constants::MSL_PRESSURE;
     float mslTemperature = Constants::MSL_TEMPERATURE;
@@ -46,28 +51,35 @@ struct ReferenceValues
     ReferenceValues(){};
 
     ReferenceValues(float altitude, float pressure, float temperature,
+                    float startLatitude  = Constants::B21_LATITUDE,
+                    float startLongitude = Constants::B21_LONGITUDE,
                     float mslPressure    = Constants::MSL_PRESSURE,
                     float mslTemperature = Constants::MSL_TEMPERATURE)
         : altitude(altitude), pressure(pressure), temperature(temperature),
+          startLatitude(startLatitude), startLongitude(startLongitude),
           mslPressure(mslPressure), mslTemperature(mslTemperature)
     {
     }
 
     static std::string header()
     {
-        return "altitude,pressure,temperature,mslPressure,mslTemperature\n";
+        return "altitude,pressure,temperature,startLatitude,startLongitude,"
+               "mslPressure,mslTemperature\n";
     }
 
     void print(std::ostream& os) const
     {
         os << altitude << "," << pressure << "," << temperature << ","
-           << mslPressure << "," << mslTemperature << "\n";
+           << startLatitude << "," << startLongitude << "," << mslPressure
+           << "," << mslTemperature << "\n";
     }
 
     bool operator==(const ReferenceValues& other) const
     {
         return altitude == other.altitude && pressure == other.pressure &&
                temperature == other.temperature &&
+               startLatitude == other.startLatitude &&
+               startLongitude == other.startLongitude &&
                mslPressure == other.mslPressure &&
                mslTemperature == other.mslTemperature;
     }
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index 6f8aaa90e..ba2aafcde 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -28,11 +28,11 @@ namespace Boardcore
 namespace Constants
 {
 
-static constexpr const float PI                 = 3.14159265f;  // [rad]
-static constexpr const float DEGREES_TO_RADIANS = PI / 180.0f;
-static constexpr const float RADIANS_TO_DEGREES = 180.0f / PI;
+static constexpr float PI                 = 3.14159265f;  // [rad]
+static constexpr float DEGREES_TO_RADIANS = PI / 180.0f;
+static constexpr float RADIANS_TO_DEGREES = 180.0f / PI;
 
-static constexpr const float g = 9.80665f;  // [m^s^2]
+static constexpr float g = 9.80665f;  // [m^s^2]
 
 static constexpr float a = 0.0065f;  // Troposphere temperature gradient [deg/m]
 static constexpr float R = 287.05f;  // Air gas constant [J/Kg/K]
@@ -44,8 +44,11 @@ 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 const float MSL_PRESSURE    = 101325.0f;  // [Pa]
-static constexpr const float MSL_TEMPERATURE = 288.15f;    // [Kelvin]
+static constexpr float MSL_PRESSURE    = 101325.0f;  // [Pa]
+static constexpr float MSL_TEMPERATURE = 288.15f;    // [Kelvin]
+
+static constexpr float B21_LATITUDE  = 45.501141;
+static constexpr float B21_LONGITUDE = 9.156281;
 
 }  // namespace Constants
 
-- 
GitLab