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

yes ofc i know what every one of these equations does

Browse Source
This commit is contained in:
Marius Eggert 2023-08-07 16:43:31 +02:00
parent 947eef7170
commit dfa20545e4
1 changed files with 44 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,15 +3,14 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <math.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <stdint.h>
#include <string.h>
#include <sys/time.h>
#include <cmath>
#include <iostream>
using namespace Math;
template <typename T1, typename T2 = T1>
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);