From d5a52eadbb85269fccb4f29acc1aa18582876aac Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:20:04 +0100 Subject: [PATCH 1/7] i should have done this ages ago --- .../coordinates/CoordinateTransformations.cpp | 59 ++++++++ .../coordinates/CoordinateTransformations.h | 6 + .../globalfunctions/math/MatrixOperations.h | 136 ++++++++++++++++++ .../globalfunctions/math/VectorOperations.h | 9 ++ 4 files changed, 210 insertions(+) diff --git a/src/fsfw/coordinates/CoordinateTransformations.cpp b/src/fsfw/coordinates/CoordinateTransformations.cpp index 322a7f0d..a983d0a2 100644 --- a/src/fsfw/coordinates/CoordinateTransformations.cpp +++ b/src/fsfw/coordinates/CoordinateTransformations.cpp @@ -6,6 +6,7 @@ #include "fsfw/globalfunctions/constants.h" #include "fsfw/globalfunctions/math/MatrixOperations.h" #include "fsfw/globalfunctions/math/VectorOperations.h" +#include "fsfw/globalfunctions/sign.h" void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition, timeval* timeUTC) { @@ -207,3 +208,61 @@ void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC, double T MatrixOperations::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); }; + +void CoordinateTransformations::cartesianFromLatLongAlt(const double lat, const double longi, + const double alt, double* cartesianOutput) { + /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, + * longitude and altitude + * @param: lat geodetic latitude [rad] + * longi longitude [rad] + * alt altitude [m] + * cartesianOutput Cartesian Coordinates in ECEF (3x1) + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff + * Landis Markley and John L. Crassidis*/ + double radiusPolar = 6356752.314; + double radiusEqua = 6378137; + + double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); + double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); + + cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); + cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); + cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); +}; + +void CoordinateTransformations::latLongAltFromCartesian(const double* vector, double& latitude, + double& longitude, double& 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 + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); + 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]); +} diff --git a/src/fsfw/coordinates/CoordinateTransformations.h b/src/fsfw/coordinates/CoordinateTransformations.h index d3760388..60dc2ab1 100644 --- a/src/fsfw/coordinates/CoordinateTransformations.h +++ b/src/fsfw/coordinates/CoordinateTransformations.h @@ -23,6 +23,12 @@ class CoordinateTransformations { static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]); + static void cartesianFromLatLongAlt(const double lat, const double longi, const double alt, + double* cartesianOutput); + + static void latLongAltFromCartesian(const double* vector, double& latitude, double& longitude, + double& altitude); + private: CoordinateTransformations(); static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, diff --git a/src/fsfw/globalfunctions/math/MatrixOperations.h b/src/fsfw/globalfunctions/math/MatrixOperations.h index 31966f8f..89c99862 100644 --- a/src/fsfw/globalfunctions/math/MatrixOperations.h +++ b/src/fsfw/globalfunctions/math/MatrixOperations.h @@ -4,6 +4,7 @@ #include #include +#include template class MatrixOperations { @@ -95,6 +96,141 @@ class MatrixOperations { } } } + + static bool isFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) { + for (uint8_t col = 0; col < cols; col++) { + for (uint8_t row = 0; row < rows; row++) { + if (not isfinite(inputMatrix[row * cols + cols])) { + return false; + } + } + } + return true; + } + + static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols, + uint8_t mainRows, uint8_t mainCols, uint8_t startRow, + uint8_t startCol) { + if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) { + return; + } + for (uint8_t row = 0; row < subRows; row++) { + for (uint8_t col = 0; col < subCols; col++) { + mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col]; + } + } + } + + static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { + // Stopwatch stopwatch; + T1 matrix[size][size], identity[size][size]; + // reformat array to matrix + for (uint8_t row = 0; row < size; row++) { + for (uint8_t col = 0; col < size; col++) { + matrix[row][col] = inputMatrix[row * size + col]; + } + } + // init identity matrix + std::memset(identity, 0.0, sizeof(identity)); + for (uint8_t diag = 0; diag < size; diag++) { + identity[diag][diag] = 1; + } + // gauss-jordan algo + // sort matrix such as no diag entry shall be 0 + for (uint8_t row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + bool swaped = false; + uint8_t rowIndex = 0; + while ((rowIndex < size) && !swaped) { + if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) { + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + swaped = true; + } + rowIndex++; + } + if (!swaped) { + return returnvalue::FAILED; // matrix not invertible + } + } + } + + for (int row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + uint8_t rowIndex; + if (row == 0) { + rowIndex = size - 1; + } else { + rowIndex = row - 1; + } + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + row--; + if (row < 0) { + return returnvalue::FAILED; // Matrix is not invertible + } + } + } + // remove non diag elements in matrix (jordan) + for (int row = 0; row < size; row++) { + for (int rowIndex = 0; rowIndex < size; rowIndex++) { + if (row != rowIndex) { + double ratio = matrix[rowIndex][row] / matrix[row][row]; + for (int colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex]; + identity[rowIndex][colIndex] -= ratio * identity[row][colIndex]; + } + } + } + } + // normalize rows in matrix (gauss) + for (int row = 0; row < size; row++) { + for (int col = 0; col < size; col++) { + identity[row][col] = identity[row][col] / matrix[row][row]; + } + } + std::memcpy(inverse, identity, sizeof(identity)); + return returnvalue::OK; // successful inversion + } + + static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { + int i, j; + double determinant = 0; + double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, + {matrix[3], matrix[4], matrix[5]}, + {matrix[6], matrix[7], matrix[8]}}; + + for (i = 0; i < 3; i++) { + determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] - + mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3])); + } + // cout<<"\n\ndeterminant: "< Date: Mon, 12 Feb 2024 14:36:14 +0100 Subject: [PATCH 2/7] fixes --- src/fsfw/globalfunctions/math/MatrixOperations.h | 4 +++- src/fsfw/globalfunctions/math/VectorOperations.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/fsfw/globalfunctions/math/MatrixOperations.h b/src/fsfw/globalfunctions/math/MatrixOperations.h index 89c99862..8d08ed90 100644 --- a/src/fsfw/globalfunctions/math/MatrixOperations.h +++ b/src/fsfw/globalfunctions/math/MatrixOperations.h @@ -1,9 +1,11 @@ #ifndef MATRIXOPERATIONS_H_ #define MATRIXOPERATIONS_H_ +#include #include #include +#include #include template @@ -100,7 +102,7 @@ class MatrixOperations { static bool isFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) { for (uint8_t col = 0; col < cols; col++) { for (uint8_t row = 0; row < rows; row++) { - if (not isfinite(inputMatrix[row * cols + cols])) { + if (not std::isfinite(inputMatrix[row * cols + cols])) { return false; } } diff --git a/src/fsfw/globalfunctions/math/VectorOperations.h b/src/fsfw/globalfunctions/math/VectorOperations.h index 6ea21db0..ca82c7f3 100644 --- a/src/fsfw/globalfunctions/math/VectorOperations.h +++ b/src/fsfw/globalfunctions/math/VectorOperations.h @@ -101,7 +101,7 @@ class VectorOperations { static bool isFinite(const T *inputVector, uint8_t size) { for (uint8_t i = 0; i < size; i++) { - if (not isfinite(inputVector[i])) { + if (not std::isfinite(inputVector[i])) { return false; } } From 9894935e9938a753cc9fcd0b00b78de0fd478985 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:42:58 +0100 Subject: [PATCH 3/7] time systems --- src/fsfw/globalfunctions/timeSystems.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 src/fsfw/globalfunctions/timeSystems.h diff --git a/src/fsfw/globalfunctions/timeSystems.h b/src/fsfw/globalfunctions/timeSystems.h new file mode 100644 index 00000000..bb03ebd3 --- /dev/null +++ b/src/fsfw/globalfunctions/timeSystems.h @@ -0,0 +1,20 @@ +#ifndef FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_ +#define FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_ + +#ifdef PLATFORM_WIN +#include +#else +#include +#endif + +class TimeSystems { + public: + virtual ~TimeSystems() {} + + static double convertUnixToJD2000(timeval time) { + // time = {{s},{us}} + return (time.tv_sec / 86400.0) + 2440587.5 - 2451545; + } +}; + +#endif /* FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_ */ From 90f3f8ef1f67dc6d07c242b68bc59c805bb407dd Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:51:54 +0100 Subject: [PATCH 4/7] removed printout --- src/fsfw/globalfunctions/math/MatrixOperations.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/fsfw/globalfunctions/math/MatrixOperations.h b/src/fsfw/globalfunctions/math/MatrixOperations.h index 8d08ed90..eefce646 100644 --- a/src/fsfw/globalfunctions/math/MatrixOperations.h +++ b/src/fsfw/globalfunctions/math/MatrixOperations.h @@ -210,8 +210,6 @@ class MatrixOperations { determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] - mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3])); } - // cout<<"\n\ndeterminant: "< Date: Fri, 23 Feb 2024 11:09:59 +0100 Subject: [PATCH 5/7] frmt --- src/fsfw/globalfunctions/math/QuaternionOperations.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/fsfw/globalfunctions/math/QuaternionOperations.cpp b/src/fsfw/globalfunctions/math/QuaternionOperations.cpp index 293eb168..780a629f 100644 --- a/src/fsfw/globalfunctions/math/QuaternionOperations.cpp +++ b/src/fsfw/globalfunctions/math/QuaternionOperations.cpp @@ -155,8 +155,7 @@ double QuaternionOperations::getAngle(const double* quaternion, bool abs) { } void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4], - const double timeDelta, - double rotRate[3]) { + const double timeDelta, double rotRate[3]) { double qOldInv[4] = {0, 0, 0, 0}; double qDelta[4] = {0, 0, 0, 0}; From bccaf4a9ea2e9d81852a4d3a2c1bd446efc593b6 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:31:11 +0100 Subject: [PATCH 6/7] capital --- src/fsfw/globalfunctions/{timeSystems.h => TimeSystems.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/fsfw/globalfunctions/{timeSystems.h => TimeSystems.h} (100%) diff --git a/src/fsfw/globalfunctions/timeSystems.h b/src/fsfw/globalfunctions/TimeSystems.h similarity index 100% rename from src/fsfw/globalfunctions/timeSystems.h rename to src/fsfw/globalfunctions/TimeSystems.h From b2576d34225f2cdb4cf89788bbd254d470ad6c20 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:33:20 +0100 Subject: [PATCH 7/7] why are we ignoring usec --- src/fsfw/globalfunctions/TimeSystems.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/fsfw/globalfunctions/TimeSystems.h b/src/fsfw/globalfunctions/TimeSystems.h index bb03ebd3..533dc5cb 100644 --- a/src/fsfw/globalfunctions/TimeSystems.h +++ b/src/fsfw/globalfunctions/TimeSystems.h @@ -12,8 +12,7 @@ class TimeSystems { virtual ~TimeSystems() {} static double convertUnixToJD2000(timeval time) { - // time = {{s},{us}} - return (time.tv_sec / 86400.0) + 2440587.5 - 2451545; + return ((time.tv_sec + time.tv_usec * 1e-6) / 86400.0) + 2440587.5 - 2451545; } };