diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index de202bf0..dd4c3f57 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -132,45 +132,25 @@ 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; - 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) {