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