From 7f3fc6d19f09070adcce080f721a36b80e025f7e Mon Sep 17 00:00:00 2001
From: "luca.conterio" <luca.conterio@skywarder.eu>
Date: Wed, 15 Sep 2021 10:40:24 +0200
Subject: [PATCH] [NAS] moved earth constants in configs

---
 .../DeathStack/NavigationAttitudeSystem/NAS.h     | 15 ---------------
 src/boards/DeathStack/configs/NASConfig.h         | 12 ++++++++++--
 2 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
index f943ee26..c863b64b 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
@@ -280,16 +280,6 @@ NASData NAS<IMU, Press, GPS>::sampleImpl()
                   nas_data.vz, nas_data.vMod);
 
         counter = 0;
-
-        /*Vector4f qua(x(6), x(7), x(8), x(9));
-        Vector3f e = quat.quat2eul(qua);
-
-        LOG_DEBUG(log,
-            "State vector: \n px: {:.2f} \n py: {:.2f} \n pz: {:.2f} \n vx:
-        {:.2f} \n " "vy: {:.2f} \n vz: {:.2f} \n roll: {:.2f} \n pitch: {:.2f}
-        \n yaw: {:.2f} \n " "q1: {:.2f} \n q2: {:.2f} \n q3: {:.2f} \n q4 :
-        {:.2f} ", x(0), x(1), x(2), x(3), x(4), x(5), e(0), e(1), e(2), x(6),
-        x(7), x(8), x(9));*/
     }
     else
     {
@@ -385,11 +375,6 @@ void NAS<IMU, Press, GPS>::updateNASData()
 template <typename IMU, typename Press, typename GPS>
 Vector3f NAS<IMU, Press, GPS>::geodetic2NED(const Vector3f& gps_data)
 {
-    const float a  = 6378137;               // [m]
-    const float a2 = pow(a, 2);             // [m^2]
-    const float b2 = pow(6356752.3142, 2);  // [m^2]
-    const float e2 = 1 - b2 / a2;
-
     float lat0 = ref_values.ref_latitude * DEGREES_TO_RADIANS;
     float lon0 = ref_values.ref_longitude * DEGREES_TO_RADIANS;
     float lat  = gps_data(0) * DEGREES_TO_RADIANS;
diff --git a/src/boards/DeathStack/configs/NASConfig.h b/src/boards/DeathStack/configs/NASConfig.h
index b64b1383..760f047f 100644
--- a/src/boards/DeathStack/configs/NASConfig.h
+++ b/src/boards/DeathStack/configs/NASConfig.h
@@ -73,6 +73,14 @@ static const float P_ATT = 0.01F;  // Attitude prediction covariance
 
 static const float P_BIAS = 0.01F;  // Bias prediction covariance
 
+/*----------------------------------------------------------------------------*/
+// Earth constants, semi-axis and eccentricity
+static constexpr float a  = 6378137;       // [m]
+static constexpr float a2 = a * a;         // [m^2]
+static constexpr float b  = 6356752.3142;  // [m]
+static constexpr float b2 = b * b;         // [m^2]
+static constexpr float e2 = 1 - b2 / a2;
+
 /*----------------------------------------------------------------------------*/
 
 // Equirectangular projection for gps: https://bit.ly/2RaMbD5
@@ -116,8 +124,8 @@ static const Vector3f NED_MAG(
 //                  // Longitude of the launch location (roccaraso)
 
 static const float EMF = 46.77F;  // [uT] micro Tesla
-                                 // Earth magnetic field, used to
-                                 // check if there's magnetic jamming
+                                  // Earth magnetic field, used to
+                                  // check if there's magnetic jamming
 
 static const Vector3f NED_MAG(
     0.5248, 0.0356, 0.8505);  // Normalized magnetic field vector at Roccaraso
-- 
GitLab