From dfa20545e48b4fa2cf0ec7f3ce03f6d451a17725 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 16:43:31 +0200 Subject: [PATCH] yes ofc i know what every one of these equations does --- mission/controller/acs/util/MathOperations.h | 51 +++++++++++++++++--- 1 file changed, 44 insertions(+), 7 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b344451a..e21a5f3a 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -3,15 +3,14 @@ #include #include -#include +#include #include #include #include +#include #include -using namespace Math; - template class MathOperations { public: @@ -114,6 +113,44 @@ class MathOperations { cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); } + + static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) { + /* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from + * cartesian coordinates in ECEF + * @param: x x-value of position vector [m] + * y y-value of position vector [m] + * z z-value of position vector [m] + * latitude geodetic latitude [rad] + * longitude longitude [rad] + * altitude altitude [m] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f + * Landis Markley and John L. Crassidis*/ + // From World Geodetic System the Earth Radii + double a = 6378137.0; // semimajor axis [m] + double b = 6356752.3142; // semiminor axis [m] + + // Calculation + double e2 = 1 - pow(b, 2) / pow(a, 2); + double epsilon2 = pow(a, 2) / pow(b, 2) - 1; + double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); + double p = std::abs(vector[2]) / epsilon2; + double s = pow(rho, 2) / (e2 * epsilon2); + double q = pow(p, 2) - pow(b, 2) + s; + double u = p / sqrt(q); + double v = pow(b, 2) * pow(u, 2) / q; + double P = 27 * v * s / q; + double Q = pow(sqrt(P + 1) + sqrt(P), 2 / 3); + double t = (1 + Q + 1 / Q) / 6; + double c = sqrt(pow(u, 2) - 1 + 2 * t); + double w = (c - u) / 2; + double d = + sign(vector[2]) * sqrt(q) * (w + pow(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4, 1 / 2)); + double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); + latitude = asin((epsilon2 + 1) * d / N); + altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; + longitude = atan2(vector[1], vector[0]); + } + static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame * @param: time Current time @@ -140,7 +177,7 @@ class MathOperations { double FloorRest = floor(rest); double secOfDay = rest - FloorRest; secOfDay *= 86400; - gmst = secOfDay / 240 * PI / 180; + gmst = secOfDay / 240 * M_PI / 180; outputDcmEJ[0] = cos(gmst); outputDcmEJ[1] = sin(gmst); @@ -191,11 +228,11 @@ class MathOperations { double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // Earth Rotation angle double era = 0; - era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); // Greenwich Mean Sidereal Time double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); - double arcsecFactor = 1 * PI / (180 * 3600); + double arcsecFactor = 1 * M_PI / (180 * 3600); gmst2000 *= arcsecFactor; gmst2000 += era; @@ -247,7 +284,7 @@ class MathOperations { double de = 9.203 * arcsecFactor * cos(Om); // % true obliquity of the ecliptic eps p.71 (simplified) - double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180; nutation[0][0] = cos(dp); nutation[1][0] = cos(e + de) * sin(dp);