eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

debug galore
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-08-08 15:06:35 +02:00
parent 8da08bd328
commit 8e2c6a95e0
1 changed files with 21 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
mission/controller/acs/util/MathOperations.h
View File

@ -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) {