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/TimeSystems.h b/src/fsfw/globalfunctions/TimeSystems.h new file mode 100644 index 00000000..533dc5cb --- /dev/null +++ b/src/fsfw/globalfunctions/TimeSystems.h @@ -0,0 +1,19 @@ +#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) { + return ((time.tv_sec + time.tv_usec * 1e-6) / 86400.0) + 2440587.5 - 2451545; + } +}; + +#endif /* FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_ */ diff --git a/src/fsfw/globalfunctions/math/MatrixOperations.h b/src/fsfw/globalfunctions/math/MatrixOperations.h index 31966f8f..eefce646 100644 --- a/src/fsfw/globalfunctions/math/MatrixOperations.h +++ b/src/fsfw/globalfunctions/math/MatrixOperations.h @@ -1,9 +1,12 @@ #ifndef MATRIXOPERATIONS_H_ #define MATRIXOPERATIONS_H_ +#include #include #include +#include +#include template class MatrixOperations { @@ -95,6 +98,139 @@ 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 std::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])); + } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) - + (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) / + determinant; + } + } + } + + static void skewMatrix(const T1 *vector, T2 *result) { + // Input Dimension [3], Output [3][3] + result[0] = 0; + result[1] = -vector[2]; + result[2] = vector[1]; + result[3] = vector[2]; + result[4] = 0; + result[5] = -vector[0]; + result[6] = -vector[1]; + result[7] = vector[0]; + result[8] = 0; + } }; #endif /* MATRIXOPERATIONS_H_ */ diff --git a/src/fsfw/globalfunctions/math/VectorOperations.h b/src/fsfw/globalfunctions/math/VectorOperations.h index b8f6b00f..ca82c7f3 100644 --- a/src/fsfw/globalfunctions/math/VectorOperations.h +++ b/src/fsfw/globalfunctions/math/VectorOperations.h @@ -99,6 +99,15 @@ class VectorOperations { static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); } + static bool isFinite(const T *inputVector, uint8_t size) { + for (uint8_t i = 0; i < size; i++) { + if (not std::isfinite(inputVector[i])) { + return false; + } + } + return true; + } + private: VectorOperations(); };