From 8e2c6a95e05553ddd7888c1d56bfc5d8efb2c6f8 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 8 Aug 2023 15:06:35 +0200 Subject: [PATCH] debug galore --- mission/controller/acs/util/MathOperations.h | 23 ++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 555f38de..6c5dfa76 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -130,25 +130,44 @@ class MathOperations { double b = 6356752.3142; // semiminor axis [m] // Calculation + sif::debug << "ECEF = [" << vector[0] << "," << vector[1] << "," << vector[2] << "]" + << std::endl; double e2 = 1 - pow(b, 2) / pow(a, 2); + sif::debug << "e2 = " << e2 << std::endl; double epsilon2 = pow(a, 2) / pow(b, 2) - 1; + sif::debug << "epsilon2 = " << epsilon2 << std::endl; double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); + sif::debug << "rho = " << rho << std::endl; double p = std::abs(vector[2]) / epsilon2; + sif::debug << "p = " << p << std::endl; double s = pow(rho, 2) / (e2 * epsilon2); + sif::debug << "s = " << s << std::endl; double q = pow(p, 2) - pow(b, 2) + s; + sif::debug << "q = " << q << std::endl; double u = p / sqrt(q); + sif::debug << "u = " << u << std::endl; double v = pow(b, 2) * pow(u, 2) / q; + sif::debug << "v = " << v << std::endl; double P = 27 * v * s / q; + sif::debug << "P = " << P << std::endl; double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); + sif::debug << "Q = " << Q << std::endl; double t = (1 + Q + 1 / Q) / 6; + sif::debug << "t = " << t << std::endl; double c = sqrt(pow(u, 2) - 1 + 2 * t); + sif::debug << "c = " << c << std::endl; double w = (c - u) / 2; - double d = - sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4)); + sif::debug << "w = " << w << std::endl; + double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4)); + sif::debug << "d = " << d << std::endl; double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); + sif::debug << "N = " << N << std::endl; latitude = asin((epsilon2 + 1) * d / N); + sif::debug << "latitude = " << latitude << std::endl; altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; + sif::debug << "altitude = " << altitude << std::endl; longitude = atan2(vector[1], vector[0]); + sif::debug << "longitude = " << longitude << std::endl; } static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {