From f4ed9810030012e1faac7925e9248c9393d05425 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 15:49:58 +0100 Subject: [PATCH 01/40] cleanup --- mission/controller/acs/util/MathOperations.h | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index dd4c3f57..c240b9ed 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -1,17 +1,10 @@ #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include #include #include -#include -#include -#include #include -#include - -#include "fsfw/serviceinterface.h" template class MathOperations { @@ -46,7 +39,7 @@ class MathOperations { static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { int min_idx; T1 temp; - memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + std::memcpy(result, matrix, rowSize * colSize * sizeof(*result)); // One by one move boundary of unsorted subarray for (int k = 0; k < rowSize; k++) { for (int i = 0; i < colSize - 1; i++) { From 13f37393862e15b9a0a795f2428723cdbf669f47 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:13:00 +0100 Subject: [PATCH 02/40] some cleanup --- mission/controller/acs/AcsParameters.h | 53 ++++++++++++++------------ 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f11a673b..4b5eef5b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -8,6 +8,9 @@ typedef unsigned char uint8_t; class AcsParameters : public HasParametersIF { + private: + static constexpr double DEG2RAD = M_PI / 180.; + public: AcsParameters(); virtual ~AcsParameters(); @@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF { struct PointingLawParameters { double zeta = 0.3; double om = 0.3; - double omMax = 1 * M_PI / 180; + double omMax = 1 * DEG2RAD; double qiMin = 0.1; double gainNullspace = 0.01; @@ -877,15 +880,15 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] // For one-axis control: uint8_t avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + double blindRotRate = 1. * DEG2RAD; } targetModeControllerParameters; struct GsTargetModeControllerParameters : PointingLawParameters { @@ -893,9 +896,9 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] } gsTargetModeControllerParameters; struct NadirModeControllerParameters : PointingLawParameters { @@ -912,7 +915,7 @@ class AcsParameters : public HasParametersIF { } inertialModeControllerParameters; struct StrParameters { - double exclusionAngle = 20 * M_PI / 180; + double exclusionAngle = 20. * DEG2RAD; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; @@ -926,25 +929,25 @@ class AcsParameters : public HasParametersIF { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of - // perigee - float m_0 = 357.5277; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis - float e1 = 0.74508 * M_PI / 180.; + float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis + float e1 = 0.74508 * DEG2RAD; - float p1 = 6892. / 3600. * M_PI / 180.; // some parameter - float p2 = 72. / 3600. * M_PI / 180.; // some parameter + float p1 = 6892. / 3600. * DEG2RAD; // some parameter + float p2 = 72. / 3600. * DEG2RAD; // some parameter } sunModelParameters; struct KalmanFilterParameters { - double sensorNoiseSTR = 0.1 * M_PI / 180; - double sensorNoiseSS = 8 * M_PI / 180; - double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseGYR = 0.1 * M_PI / 180; + double sensorNoiseStr = 0.1 * DEG2RAD; + double sensorNoiseSus = 8. * DEG2RAD; + double sensorNoiseMgm = 4. * DEG2RAD; + double sensorNoiseGyr = 0.1 * DEG2RAD; - double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk - double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability + double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk + double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability } kalmanFilterParameters; struct MagnetorquerParameter { @@ -960,8 +963,8 @@ class AcsParameters : public HasParametersIF { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 1 * M_PI / 180; + double omegaDetumbleStart = 2 * DEG2RAD; + double omegaDetumbleEnd = 1 * DEG2RAD; double gainBdot = pow(10.0, -3.3); double gainFull = pow(10.0, -2.3); uint8_t useFullDetumbleLaw = false; From 883ff4f6de62d4f0f87b34156ccfb9ed07795c57 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:18:12 +0100 Subject: [PATCH 03/40] never needed this anyways --- .../acs/util/CholeskyDecomposition.h | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 mission/controller/acs/util/CholeskyDecomposition.h diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h deleted file mode 100644 index 667f9f63..00000000 --- a/mission/controller/acs/util/CholeskyDecomposition.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * TinyEKF: Extended Kalman Filter for embedded processors - * - * Copyright (C) 2015 Simon D. Levy - * - * MIT License - */ -#ifndef CHOLESKYDECOMPOSITION_H_ -#define CHOLESKYDECOMPOSITION_H_ -#include -// typedef unsigned int uint8_t; - -template -class CholeskyDecomposition { - public: - static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) { - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - return cholsl(matrix, result, tempMatrix, dimension); - } - - private: - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t choldc1(double *a, double *p, uint8_t n) { - int8_t i, j, k; - double sum; - - for (i = 0; i < n; i++) { - for (j = i; j < n; j++) { - sum = a[i * n + j]; - for (k = i - 1; k >= 0; k--) { - sum -= a[i * n + k] * a[j * n + k]; - } - if (i == j) { - if (sum <= 0) { - return 1; /* error */ - } - p[i] = sqrt(sum); - } else { - a[j * n + i] = sum / p[i]; - } - } - } - - return 0; /* success */ - } - - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) { - uint8_t i, j, k; - double sum; - for (i = 0; i < n; i++) - for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j]; - if (choldc1(a, p, n)) return 1; - for (i = 0; i < n; i++) { - a[i * n + i] = 1 / p[i]; - for (j = i + 1; j < n; j++) { - sum = 0; - for (k = i; k < j; k++) { - sum -= a[j * n + k] * a[k * n + i]; - } - a[j * n + i] = sum / p[j]; - } - } - - return 0; /* success */ - } - - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) { - uint8_t i, j, k; - if (choldcsl(A, a, p, n)) return 1; - for (i = 0; i < n; i++) { - for (j = i + 1; j < n; j++) { - a[i * n + j] = 0.0; - } - } - for (i = 0; i < n; i++) { - a[i * n + i] *= a[i * n + i]; - for (k = i + 1; k < n; k++) { - a[i * n + i] += a[k * n + i] * a[k * n + i]; - } - for (j = i + 1; j < n; j++) { - for (k = j; k < n; k++) { - a[i * n + j] += a[k * n + i] * a[k * n + j]; - } - } - } - for (i = 0; i < n; i++) { - for (j = 0; j < i; j++) { - a[i * n + j] = a[j * n + i]; - } - } - - return 0; /* success */ - } -}; - -#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */ From 4e8776ff6861288c50fa9c07fd9ef414c3ce7faa Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:19:02 +0100 Subject: [PATCH 04/40] i shouldnt be doing this --- mission/controller/acs/util/MathOperations.h | 29 ++++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index c240b9ed..7bf7319b 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -6,6 +6,10 @@ #include +// Most of these functions already exist within the FSFW and are redundant +// All others should have been merged into the FSFW +// So if you are that bored, that you are reading this, you were better of merging these now + template class MathOperations { public: @@ -361,7 +365,7 @@ class MathOperations { return det; } - static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { + 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 @@ -392,7 +396,7 @@ class MathOperations { rowIndex++; } if (!swaped) { - return 1; // matrix not invertible + return returnvalue::FAILED; // matrix not invertible } } } @@ -411,7 +415,7 @@ class MathOperations { } row--; if (row < 0) { - return 1; // Matrix is not invertible + return returnvalue::FAILED; // Matrix is not invertible } } } @@ -434,12 +438,12 @@ class MathOperations { } } std::memcpy(inverse, identity, sizeof(identity)); - return 0; // successful inversion + return returnvalue::OK; // successful inversion } static bool checkVectorIsFinite(const T1 *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; } } @@ -449,13 +453,26 @@ class MathOperations { static bool checkMatrixIsFinite(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 + col])) { 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]; + } + } + } }; #endif /* ACS_MATH_MATHOPERATIONS_H_ */ From ff76f99ad9e673e6e577f2ff26353f2d26ca5e1c Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 7 Feb 2024 10:00:32 +0100 Subject: [PATCH 05/40] some fun with parameters --- mission/controller/acs/AcsParameters.h | 56 +++++++++++++------------- 1 file changed, 29 insertions(+), 27 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 09804612..f4a1f309 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -22,7 +22,7 @@ class AcsParameters : public HasParametersIF { uint8_t fusedRateSafeDuringEclipse = true; uint8_t fusedRateFromStr = false; uint8_t fusedRateFromQuest = false; - double questFilterWeight = 0.0; + double questFilterWeight = 0.9; } onBoardParams; struct InertiaEIVE { @@ -854,7 +854,7 @@ class AcsParameters : public HasParametersIF { struct PointingLawParameters { double zeta = 0.3; double om = 0.3; - double omMax = 1 * M_PI / 180; + double omMax = 1 * DEG2RAD; double qiMin = 0.1; double gainNullspace = 0.01; @@ -876,15 +876,15 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] // For one-axis control: uint8_t avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + double blindRotRate = 1 * DEG2RAD; } targetModeControllerParameters; struct GsTargetModeControllerParameters : PointingLawParameters { @@ -892,9 +892,9 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] } gsTargetModeControllerParameters; struct NadirModeControllerParameters : PointingLawParameters { @@ -911,7 +911,7 @@ class AcsParameters : public HasParametersIF { } inertialModeControllerParameters; struct StrParameters { - double exclusionAngle = 20 * M_PI / 180; + double exclusionAngle = 20 * DEG2RAD; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; @@ -925,25 +925,25 @@ class AcsParameters : public HasParametersIF { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of - // perigee - float m_0 = 357.5277; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis - float e1 = 0.74508 * M_PI / 180.; + float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis + float e1 = 0.74508 * DEG2RAD; - float p1 = 6892. / 3600. * M_PI / 180.; // some parameter - float p2 = 72. / 3600. * M_PI / 180.; // some parameter + float p1 = 6892. / 3600. * DEG2RAD; // some parameter + float p2 = 72. / 3600. * DEG2RAD; // some parameter } sunModelParameters; struct KalmanFilterParameters { - double sensorNoiseSTR = 0.1 * M_PI / 180; - double sensorNoiseSS = 8 * M_PI / 180; - double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseGYR = 0.1 * M_PI / 180; + double sensorNoiseSTR = 0.1 * DEG2RAD; + double sensorNoiseSS = 8 * DEG2RAD; + double sensorNoiseMAG = 4 * DEG2RAD; + double sensorNoiseGYR = 0.1 * DEG2RAD; - double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk - double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability + double sensorNoiseArwGYR = 3 * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk + double sensorNoiseBsGYR = 3 * DEG2RAD / 3600; // Bias Stability } kalmanFilterParameters; struct MagnetorquerParameter { @@ -959,12 +959,14 @@ class AcsParameters : public HasParametersIF { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 1 * M_PI / 180; + double omegaDetumbleStart = 2 * DEG2RAD; + double omegaDetumbleEnd = 1 * DEG2RAD; double gainBdot = pow(10.0, -3.3); double gainFull = pow(10.0, -2.3); uint8_t useFullDetumbleLaw = false; } detumbleParameter; -}; + private: + static constexpr double DEG2RAD = M_PI / 180.; +}; #endif /* ACSPARAMETERS_H_ */ From c064d1f6256a92111908d0472e4efc0803408141 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 11:09:14 +0100 Subject: [PATCH 06/40] ??? --- mission/controller/acs/Guidance.cpp | 100 ++++++++++++++-------------- 1 file changed, 50 insertions(+), 50 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 3641e41e..ef21f7fc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -8,8 +8,8 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]) { // positive z-Axis of EIVE in direction of sun - double zAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirI, zAxisXI, 3); + double zAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(sunDirI, zAxisIX, 3); // determine helper vector to point x-Axis and therefore the STR away from Earth double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; @@ -17,20 +17,20 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, VectorOperations::normalize(posSatI, helperXI, 3); // construct y-axis from helper vector and z-axis - double yAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(zAxisXI, helperXI, yAxisXI); - VectorOperations::normalize(yAxisXI, yAxisXI, 3); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(zAxisIX, helperXI, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // x-axis completes RHS - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); - VectorOperations::normalize(xAxisXI, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(yAxisIX, zAxisIX, xAxisIX); + VectorOperations::normalize(xAxisIX, xAxisIX, 3); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); // calculate of reference rotation rate targetRotationRate(timeDelta, targetQuat, targetSatRotRate); @@ -59,8 +59,8 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirI, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirI, xAxisIX, 3); // transform velocity into inertial frame double velSatI[3] = {0, 0, 0}; @@ -73,18 +73,18 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture // resolution - double yAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalI, xAxisXI, yAxisXI); - VectorOperations::normalize(yAxisXI, yAxisXI, 3); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalI, xAxisIX, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // z-axis completes RHS - double zAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(xAxisXI, yAxisXI, zAxisXI); + double zAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(xAxisIX, yAxisIX, zAxisIX); // join transformation matrix - double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); @@ -111,32 +111,32 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(groundStationDirI, xAxisXI, 3); - VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(groundStationDirI, xAxisIX, 3); + VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 3); // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxisXI, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxisXI, 3), 2); - double sunParallel[3], zAxisXI[3]; - VectorOperations::mulScalar(xAxisXI, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxisXI, 3); - VectorOperations::normalize(zAxisXI, zAxisXI, 3); + double xDotS = VectorOperations::dot(xAxisIX, sunDirI); + xDotS /= pow(VectorOperations::norm(xAxisIX, 3), 2); + double sunParallel[3], zAxisIX[3]; + VectorOperations::mulScalar(xAxisIX, xDotS, sunParallel, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxisIX, 3); + VectorOperations::normalize(zAxisIX, zAxisIX, 3); // y-axis completes RHS - double yAxisXI[3]; - VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); - VectorOperations::normalize(yAxisXI, yAxisXI, 3); + double yAxisIX[3]; + VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } @@ -153,26 +153,26 @@ void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, // negative x-axis aligned with position vector // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(posSatI, xAxisXI, 3); - VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, xAxisIX, 3); + VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); // make z-Axis parallel to major part of camera resolution - double zAxisXI[3] = {0, 0, 0}; + double zAxisIX[3] = {0, 0, 0}; double velSatI[3] = {0, 0, 0}; CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute); - VectorOperations::cross(xAxisXI, velSatI, zAxisXI); - VectorOperations::normalize(zAxisXI, zAxisXI, 3); + VectorOperations::cross(xAxisIX, velSatI, zAxisIX); + VectorOperations::normalize(zAxisIX, zAxisIX, 3); // y-Axis completes RHS - double yAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, refSatRate); } From b68bbe64a3834f4b88570fe1fde0d4f8fdadd78a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:43:17 +0100 Subject: [PATCH 07/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index b5e7179a..9894935e 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit b5e7179af1da085b9be598b4897f2664012781af +Subproject commit 9894935e9938a753cc9fcd0b00b78de0fd478985 From 236ca64de3bbc47a9fe1848820b83efb382bd56f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:43:34 +0100 Subject: [PATCH 08/40] whole lot of cleanup --- mission/controller/acs/AcsParameters.cpp | 12 +- mission/controller/acs/AttitudeEstimation.cpp | 4 +- mission/controller/acs/Guidance.cpp | 4 +- mission/controller/acs/Guidance.h | 1 - mission/controller/acs/Igrf13Model.cpp | 22 +- mission/controller/acs/Igrf13Model.h | 9 +- .../acs/MultiplicativeKalmanFilter.cpp | 36 +- mission/controller/acs/Navigation.cpp | 3 - mission/controller/acs/SensorProcessing.cpp | 8 +- mission/controller/acs/SensorProcessing.h | 3 +- mission/controller/acs/util/MathOperations.h | 478 ------------------ 11 files changed, 42 insertions(+), 538 deletions(-) delete mode 100644 mission/controller/acs/util/MathOperations.h diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 8816eed9..f8fb759b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x11): // KalmanFilterParameters switch (parameterId) { case 0x0: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr); break; case 0x1: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus); break; case 0x2: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm); break; case 0x3: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr); break; case 0x4: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw); break; case 0x5: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 3e4a22c4..3b6bf181 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, // Sensor Weights double kSus = 0, kMgm = 0; - kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); - kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); + kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2); + kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2); // Weighted Vectors double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index ef21f7fc..b167573a 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -44,7 +44,7 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetF[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( + CoordinateTransformations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.altitudeTgt, targetF); @@ -98,7 +98,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double posGroundStationF[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( + CoordinateTransformations::cartesianFromLatLongAlt( acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 9835b762..c9ae1b07 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 3f844434..4ae7c93a 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -1,19 +1,5 @@ #include "Igrf13Model.h" -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "util/MathOperations.h" - -using namespace Math; - Igrf13Model::Igrf13Model() {} Igrf13Model::~Igrf13Model() {} @@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double magFieldModel[3] = {0, 0, 0}; double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ - theta -= 90 * PI / 180; + theta -= 90. * M_PI / 180.; theta *= (-1); double rE = 6371200.0; // radius earth [m] @@ -83,13 +69,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement); double UT1 = JD2000 / 36525.; double gst = 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); gst = std::fmod(gst, 360.); - gst *= PI / 180.; + gst *= M_PI / 180.; double lst = gst + longitude; // local sidereal time [rad] magFieldModelInertial[0] = @@ -107,7 +93,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement); double days = ceil(JD2000 - JD2000Igrf); for (int i = 0; i <= igrfOrder; i++) { for (int j = 0; j <= (igrfOrder - 1); j++) { diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 187adde7..774d6c58 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,10 +16,11 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index ba878034..efa74568 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -9,9 +9,6 @@ #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} @@ -25,9 +22,9 @@ ReturnValue_t MultiplicativeKalmanFilter::init( if (validMagField_ && validSS && validSSModel && validMagModel) { // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; - sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; + sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -234,9 +231,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // If we are here, MEKF will perform double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; - sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; + sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -264,8 +261,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str - MathOperations::skewMatrix(sunEstB, *measSensMatrix11); - MathOperations::skewMatrix(magEstB, *measSensMatrix22); + MatrixOperations::skewMatrix(sunEstB, *measSensMatrix11); + MatrixOperations::skewMatrix(magEstB, *measSensMatrix22); double measVecQuat[3] = {0, 0, 0}; if (validSTR_) { @@ -837,8 +834,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> double invResidualCov[MDF][MDF] = {{0}}; - int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); - if (inversionFailed) { + ReturnValue_t result = + MatrixOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); + if (result != returnvalue::OK) { updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } @@ -874,7 +872,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // State Vector Elements double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::skewMatrix(propagatedQuaternion, *xi2); + MatrixOperations::skewMatrix(propagatedQuaternion, *xi2); double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); @@ -898,8 +896,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; - double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; + double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs; + double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -1057,7 +1055,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::skewMatrix(rotSin, *skewSin); + MatrixOperations::skewMatrix(rotSin, *skewSin); MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -1080,8 +1078,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - if (not(MathOperations::checkVectorIsFinite(propagatedQuaternion, 4)) || - not(MathOperations::checkMatrixIsFinite(initialQuaternion, 6, 6))) { + if (not(VectorOperations::isFinite(propagatedQuaternion, 4)) || + not(MatrixOperations::isFinite(initialQuaternion, 6, 6))) { updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); return MEKF_NOT_FINITE; } diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 624f4cea..5990eca1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -5,9 +5,6 @@ #include #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - Navigation::Navigation() {} Navigation::~Navigation() {} diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 43582f18..61292c36 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -180,7 +180,7 @@ void SensorProcessing::processSus( const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { /* -------- Sun Model Direction (IJK frame) ------- */ - double JD2000 = MathOperations::convertUnixToJD2000(timeAbsolute); + double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute); // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; @@ -528,8 +528,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong uint8_t gpsSource = acs::gps::Source::NONE; // We do not trust the GPS and therefore it shall die here if SPG4 is running if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { - MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, - gdLongitude, altitude); + CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, + gdLatitude, gdLongitude, altitude); double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(gdLatitude)); { @@ -559,7 +559,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); + CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index fea0fd01..35d3ae6d 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -2,11 +2,13 @@ #define SENSORPROCESSING_H_ #include +#include #include #include #include #include #include +#include #include #include #include @@ -14,7 +16,6 @@ #include #include #include -#include #include #include diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h deleted file mode 100644 index 7bf7319b..00000000 --- a/mission/controller/acs/util/MathOperations.h +++ /dev/null @@ -1,478 +0,0 @@ -#ifndef MATH_MATHOPERATIONS_H_ -#define MATH_MATHOPERATIONS_H_ - -#include -#include - -#include - -// Most of these functions already exist within the FSFW and are redundant -// All others should have been merged into the FSFW -// So if you are that bored, that you are reading this, you were better of merging these now - -template -class MathOperations { - public: - 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; - } - static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result, - uint8_t size = 3) { - // Looks like MatrixOpertions::multiply is able to do the same thing - for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { - for (uint8_t resultRow = 0; resultRow < size; resultRow++) { - result[resultColumn + size * resultRow] = - vector1[resultRow] * transposeVector2[resultColumn]; - } - } - /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; - matrixMag[i][j] = magEstB[i] * magEstB[j]; - matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; - matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ - } - - static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { - int min_idx; - T1 temp; - std::memcpy(result, matrix, rowSize * colSize * sizeof(*result)); - // One by one move boundary of unsorted subarray - for (int k = 0; k < rowSize; k++) { - for (int i = 0; i < colSize - 1; i++) { - // Find the minimum element in unsorted array - min_idx = i; - for (int j = i + 1; j < colSize; j++) { - if (result[j + k * colSize] < result[min_idx + k * colSize]) { - min_idx = j; - } - } - // Swap the found minimum element with the first element - temp = result[i + k * colSize]; - result[i + k * colSize] = result[min_idx + k * colSize]; - result[min_idx + k * colSize] = temp; - } - } - } - - static void convertDateToJD2000(const T1 time, T2 julianDate) { - // time = { Y, M, D, h, m,s} - // time in sec and microsec -> The Epoch (unixtime) - julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) + - floor(275 * time[1] / 9) + time[2] + - (60 * time[3] + time[4] + (time(5) / 60)) / 1440; - } - - static T1 convertUnixToJD2000(timeval time) { - // time = {{s},{us}} - T1 julianDate2000; - julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545; - return julianDate2000; - } - - static void dcmFromQuat(const T1 vector[], T1 *outputDcm) { - // convention q = [qx,qy,qz, qw] - outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); - outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]); - outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]); - - outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]); - outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); - outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]); - - outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]); - outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]); - outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2); - } - - static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, - T2 *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); - } - - 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 + 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]); - } - - static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - double JD2000Floor = 0; - double JD2000 = convertUnixToJD2000(time); - // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) - JD2000Floor = floor(JD2000); - if ((JD2000 - JD2000Floor) < 0.5) { - JD2000Floor -= 0.5; - } else { - JD2000Floor += 0.5; - } - - double JC2000 = JD2000Floor / 36525; - double sec = (JD2000 - JD2000Floor) * 86400; - double gmst = 0; // greenwich mean sidereal time - gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) - - 0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec; - double rest = gmst / 86400; - double FloorRest = floor(rest); - double secOfDay = rest - FloorRest; - secOfDay *= 86400; - gmst = secOfDay / 240 * M_PI / 180; - - outputDcmEJ[0] = cos(gmst); - outputDcmEJ[1] = sin(gmst); - outputDcmEJ[2] = 0; - outputDcmEJ[3] = -sin(gmst); - outputDcmEJ[4] = cos(gmst); - outputDcmEJ[5] = 0; - outputDcmEJ[6] = 0; - outputDcmEJ[7] = 0; - outputDcmEJ[8] = 1; - - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION - double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, - {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, - {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = 0.000072921158553; - double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); - MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); - } - - /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame - * give also the back the derivative of this matrix - * @param: unixTime Current time in Unix format - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und - Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff - * Oliver Zeile - * - https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ - static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { - // TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 - //(initial Offset) International Atomic Time (TAI) - - double JD2000UTC1 = convertUnixToJD2000(unixTime); - - // Julian Date / century from TT - timeval terestrialTime = unixTime; - terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; - double JD2000TT = convertUnixToJD2000(terestrialTime); - double JC2000TT = JD2000TT / 36525; - - //------------------------------------------------------------------------------------- - // Calculation of Transformation from earth rotation Theta - //------------------------------------------------------------------------------------- - double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - // Earth Rotation angle - double era = 0; - 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 * M_PI / (180 * 3600); - gmst2000 *= arcsecFactor; - gmst2000 += era; - - theta[0][0] = cos(gmst2000); - theta[0][1] = sin(gmst2000); - theta[0][2] = 0; - theta[1][0] = -sin(gmst2000); - theta[1][1] = cos(gmst2000); - theta[1][2] = 0; - theta[2][0] = 0; - theta[2][1] = 0; - theta[2][2] = 1; - - //------------------------------------------------------------------------------------- - // Calculation of Transformation from earth Precession P - //------------------------------------------------------------------------------------- - double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3); - double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3); - double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3); - - zeta *= arcsecFactor; - theta2 *= arcsecFactor; - ze *= arcsecFactor; - - precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta); - precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta); - precession[2][0] = sin(theta2) * cos(zeta); - precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta); - precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta); - precession[2][1] = -sin(theta2) * sin(zeta); - precession[0][2] = -cos(ze) * sin(theta2); - precession[1][2] = -sin(ze) * sin(theta2); - precession[2][2] = cos(theta2); - - //------------------------------------------------------------------------------------- - // Calculation of Transformation from earth Nutation N - //------------------------------------------------------------------------------------- - double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - // lunar asc node - double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + - 7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3); - Om *= arcsecFactor; - // delta psi approx - double dp = -17.2 * arcsecFactor * sin(Om); - - // delta eps approx - double de = 9.203 * arcsecFactor * cos(Om); - - // % true obliquity of the ecliptic eps p.71 (simplified) - 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); - nutation[2][0] = sin(e + de) * sin(dp); - nutation[0][1] = -cos(e) * sin(dp); - nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de); - nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de); - nutation[0][2] = -sin(e) * sin(dp); - nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de); - nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de); - - //------------------------------------------------------------------------------------- - // Calculation of Derivative of rotation matrix from earth - //------------------------------------------------------------------------------------- - double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = 0.000072921158553; - MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - - //------------------------------------------------------------------------------------- - // Calculation of transformation matrix and Derivative of transformation matrix - //------------------------------------------------------------------------------------- - double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); - MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); - - MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); - } - 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: "<::matrixDeterminant(*submatrix, size - 1)); - } - } - return det; - } - - 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 bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) { - for (uint8_t i = 0; i < size; i++) { - if (not std::isfinite(inputVector[i])) { - return false; - } - } - return true; - } - - static bool checkMatrixIsFinite(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 + col])) { - 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]; - } - } - } -}; - -#endif /* ACS_MATH_MATHOPERATIONS_H_ */ From d32f7b31c355c344e29acbc981eb445fc897dcfc Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 13 Feb 2024 15:58:18 +0100 Subject: [PATCH 09/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 9894935e..90f3f8ef 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9894935e9938a753cc9fcd0b00b78de0fd478985 +Subproject commit 90f3f8ef1f67dc6d07c242b68bc59c805bb407dd From 16255a91dc6d60f4c0b7ec63e5b09924ec65010b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 13 Feb 2024 15:58:44 +0100 Subject: [PATCH 10/40] gs pointing with sun/earth avoidance --- mission/controller/acs/Guidance.cpp | 74 +++++++++++++++++++++++------ mission/controller/acs/Guidance.h | 4 +- 2 files changed, 62 insertions(+), 16 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index b167573a..7a630eef 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -90,9 +90,9 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, + const double posSatF[3], const double sunDirI[3], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -115,20 +115,66 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou VectorOperations::normalize(groundStationDirI, xAxisIX, 3); VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); - // get sun vector model in ECI - VectorOperations::normalize(sunDirI, sunDirI, 3); + // normalize sun vector in ECI + double sunDirNormalizedI[3] = {0, 0, 0}; + // get earth vector in ECI + double earthDirNormalizedI[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, earthDirNormalizedI, 3); + VectorOperations::mulScalar(earthDirNormalizedI, -1, earthDirNormalizedI, 3); - // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector - // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxisIX, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxisIX, 3), 2); - double sunParallel[3], zAxisIX[3]; - VectorOperations::mulScalar(xAxisIX, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxisIX, 3); + // sun avoidance calculations + double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}; + VectorOperations::mulScalar( + xAxisIX, VectorOperations::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3); + VectorOperations::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3); + VectorOperations::normalize(sunFloorYZ, sunFloorYZ, 3); + double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, + strVecSunZ[3] = {0, 0, 0}; + VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], + strVecSunX, 3); + VectorOperations::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2], + strVecSunZ, 3); + VectorOperations::add(strVecSunX, strVecSunZ, strVecSun, 3); + VectorOperations::normalize(strVecSun, strVecSun, 3); + sunWeight = VectorOperations::dot(strVecSun, sunDirNormalizedI); + + // earth avoidance calculations + double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}; + VectorOperations::mulScalar( + xAxisIX, VectorOperations::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3); + VectorOperations::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3); + VectorOperations::normalize(earthFloorYZ, earthFloorYZ, 3); + double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, + strVecEarthZ[3] = {0, 0, 0}; + VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], + strVecEarthX, 3); + VectorOperations::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2], + strVecEarthZ, 3); + VectorOperations::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); + VectorOperations::normalize(strVecEarth, strVecEarth, 3); + earthWeight = VectorOperations::dot(strVecEarth, earthDirNormalizedI); + + if ((sunWeight == 0.0) and (earthWeight == 0.0)) { + // if this actually ever happens i will eat a broom + sunWeight = 0.5; + earthWeight = 0.5; + } + + // normalize weights for convenience + double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight)); + sunWeight *= normFactor; + earthWeight *= normFactor; + + // calculate z-axis for str blinding avoidance + double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0}; + VectorOperations::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3); + VectorOperations::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3); + VectorOperations::add(zAxisSun, zAxisEarth, zAxisIX, 3); + VectorOperations::mulScalar(zAxisIX, -1, zAxisIX, 3); VectorOperations::normalize(zAxisIX, zAxisIX, 3); - // y-axis completes RHS - double yAxisIX[3]; + // calculate y-axis + double yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); VectorOperations::normalize(yAxisIX, yAxisIX, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index c9ae1b07..6fbce0a4 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -28,8 +28,8 @@ class Guidance { const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], double velSatE[3], double quatIX[4], double targetSatRotRate[3]); - void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3], + const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], double velSatF[3], double targetQuat[4], double refSatRate[3]); From 9d6206302ade41ab92c64b912295f036cf99be42 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 13 Feb 2024 16:09:35 +0100 Subject: [PATCH 11/40] changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0705dae2..ddee6afa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Bumped `eive-fsfw` + ## Fixed - PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report @@ -24,6 +26,11 @@ will consitute of a breaking change warranting a new major release: - Bugfix in PLOC SUPV latchup report parsing. - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid. +## Changed + +- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw` +- Changed pointing strategy for target groundstation mode to prevent blinding of the STR + # [v7.6.1] 2024-02-05 ## Changed From f60fe1ed02a5d3fdd8b74ed1786f38e75aa1c6ed Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 13 Feb 2024 16:58:40 +0100 Subject: [PATCH 12/40] added normalization of sun model vector --- mission/controller/acs/SensorProcessing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 61292c36..ac82e891 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -198,6 +198,7 @@ void SensorProcessing::processSus( sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); + VectorOperations::normalize(sunIjkModel, sunIjkModel, 3); uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; if (sus0valid) { From b9e4c51d826471635f2aeced2012218e9d0ba231 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 13 Feb 2024 16:59:50 +0100 Subject: [PATCH 13/40] set stuff const that should be const --- mission/controller/acs/Guidance.cpp | 35 ++++++++++++++--------------- mission/controller/acs/Guidance.h | 9 ++++---- 2 files changed, 21 insertions(+), 23 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7a630eef..cc0a20d9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -36,9 +36,9 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double velSatF[3], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, + const double posSatF[3], const double velSatF[3], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -115,18 +115,16 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, VectorOperations::normalize(groundStationDirI, xAxisIX, 3); VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); - // normalize sun vector in ECI - double sunDirNormalizedI[3] = {0, 0, 0}; // get earth vector in ECI - double earthDirNormalizedI[3] = {0, 0, 0}; - VectorOperations::normalize(posSatI, earthDirNormalizedI, 3); - VectorOperations::mulScalar(earthDirNormalizedI, -1, earthDirNormalizedI, 3); + double earthDirI[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, earthDirI, 3); + VectorOperations::mulScalar(earthDirI, -1, earthDirI, 3); // sun avoidance calculations double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}; - VectorOperations::mulScalar( - xAxisIX, VectorOperations::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3); - VectorOperations::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3); + VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, sunDirI), + sunPerpendicularX, 3); + VectorOperations::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3); VectorOperations::normalize(sunFloorYZ, sunFloorYZ, 3); double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, strVecSunZ[3] = {0, 0, 0}; @@ -136,13 +134,13 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, strVecSunZ, 3); VectorOperations::add(strVecSunX, strVecSunZ, strVecSun, 3); VectorOperations::normalize(strVecSun, strVecSun, 3); - sunWeight = VectorOperations::dot(strVecSun, sunDirNormalizedI); + sunWeight = VectorOperations::dot(strVecSun, sunDirI); // earth avoidance calculations double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}; - VectorOperations::mulScalar( - xAxisIX, VectorOperations::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3); - VectorOperations::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3); + VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, earthDirI), + earthPerpendicularX, 3); + VectorOperations::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3); VectorOperations::normalize(earthFloorYZ, earthFloorYZ, 3); double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, strVecEarthZ[3] = {0, 0, 0}; @@ -152,7 +150,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, strVecEarthZ, 3); VectorOperations::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); VectorOperations::normalize(strVecEarth, strVecEarth, 3); - earthWeight = VectorOperations::dot(strVecEarth, earthDirNormalizedI); + earthWeight = VectorOperations::dot(strVecEarth, earthDirI); if ((sunWeight == 0.0) and (earthWeight == 0.0)) { // if this actually ever happens i will eat a broom @@ -187,8 +185,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3], - double velSatE[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, + const double posSatE[3], const double velSatE[3], + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 6fbce0a4..5284ba01 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -9,7 +9,6 @@ #include #include #include -#include #include #include @@ -26,12 +25,12 @@ class Guidance { void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); - void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double velSatE[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3], + const double velSatE[3], double quatIX[4], double targetSatRotRate[3]); void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3], const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double velSatF[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3], + const double velSatF[3], double targetQuat[4], double refSatRate[3]); void targetRotationRate(const double timeDelta, double quatInertialTarget[4], double *targetSatRotRate); From 124ae6cc7eaa738452aefa153f57083140739047 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 14 Feb 2024 09:04:56 +0100 Subject: [PATCH 14/40] as i dont like being surprised by implicit stuff ... --- mission/controller/acs/Guidance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cc0a20d9..538d4e35 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -159,7 +159,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, } // normalize weights for convenience - double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight)); + double normFactor = 1. / (std::fabs(sunWeight) + std::fabs(earthWeight)); sunWeight *= normFactor; earthWeight *= normFactor; From 9124cb85fa62e38228329d065de389842bfef0b9 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 14 Feb 2024 09:06:39 +0100 Subject: [PATCH 15/40] nvm this is since c++ 23 lol --- mission/controller/acs/Guidance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 538d4e35..cc0a20d9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -159,7 +159,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, } // normalize weights for convenience - double normFactor = 1. / (std::fabs(sunWeight) + std::fabs(earthWeight)); + double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight)); sunWeight *= normFactor; earthWeight *= normFactor; From 2b09ec8bc03a5a4e14977c6cf810ddec026b79d0 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 14 Feb 2024 10:09:39 +0100 Subject: [PATCH 16/40] fixes --- mission/controller/acs/Guidance.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cc0a20d9..2972dfed 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -48,8 +48,6 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.altitudeTgt, targetF); - double targetDirF[3] = {0, 0, 0}; - VectorOperations::subtract(targetF, posSatF, targetDirF, 3); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; @@ -106,7 +104,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); - CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute); VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction From 43cca9ed0c408e7dacd76f31f7c763edd4c7f282 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Feb 2024 10:36:53 +0100 Subject: [PATCH 17/40] PSB update --- CHANGELOG.md | 5 +++++ fsfw | 2 +- mission/genericFactory.cpp | 26 ++++++++++++++++++-------- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0705dae2..4710538c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,11 @@ will consitute of a breaking change warranting a new major release: - Bugfix in PLOC SUPV latchup report parsing. - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid. +## Changed + +- Increased message queue depth and maximum number of handled messages per cycle for + `PusServiceBase` based classes (especially PUS scheduler). + # [v7.6.1] 2024-02-05 ## Changed diff --git a/fsfw b/fsfw index b5e7179a..cd2cd61b 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit b5e7179af1da085b9be598b4897f2664012781af +Subproject commit cd2cd61ba575603f53178d649a3fe344ddbcaa55 diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 47cb1a5f..daaf59cc 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -250,20 +250,30 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16); - new Service5EventReporting( - PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), - 80, 160); + + auto psbParamsService5 = + PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5); + psbParamsService5.requestQueueDepth = 50; + psbParamsService5.maxPacketsPerCycle = 50; + new Service5EventReporting(psbParamsService5, 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9)); - new Service11TelecommandScheduling( - PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11), - ccsdsDistrib); + auto psbParamsService11 = + PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11); + psbParamsService11.requestQueueDepth = 100; + psbParamsService11.maxPacketsPerCycle = 100; + new Service11TelecommandScheduling(psbParamsService11, + ccsdsDistrib); + new Service15TmStorage(objects::PUS_SERVICE_15_TM_STORAGE, config::EIVE_PUS_APID, 10); - new Service17Test( - PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17)); + auto psbParamsService17 = + PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17); + psbParamsService17.requestQueueDepth = 50; + psbParamsService17.maxPacketsPerCycle = 50; + new Service17Test(psbParamsService17); new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID, pus::PUS_SERVICE_20); new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID, From dbb530e27bfa9dfbc25bf172cfa88bd167680412 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Feb 2024 17:16:08 +0100 Subject: [PATCH 18/40] Unlock STR second firmware slot --- bsp_q7s/objectFactory.cpp | 2 +- linux/acs/StrComHandler.cpp | 42 +++++---- linux/acs/StrComHandler.h | 5 +- mission/acs/str/StarTrackerHandler.cpp | 124 +++++++++++++++++++------ mission/acs/str/StarTrackerHandler.h | 18 ++-- mission/acs/str/strHelpers.h | 21 ++++- tmtc | 2 +- 7 files changed, 158 insertions(+), 56 deletions(-) diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index 313c880d..c0dc489a 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -951,7 +951,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManage auto cfgGetter = new StrConfigPathGetter(sdcMan); auto starTracker = new StarTrackerHandler(objects::STAR_TRACKER, objects::STR_COM_IF, starTrackerCookie, - strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter); + strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter, sdcMan); starTracker->setPowerSwitcher(pwrSwitcher); starTracker->connectModeTreeParent(*strAssy); starTracker->setCustomFdir(strFdir); diff --git a/linux/acs/StrComHandler.cpp b/linux/acs/StrComHandler.cpp index 0bb4d749..a02d47d1 100644 --- a/linux/acs/StrComHandler.cpp +++ b/linux/acs/StrComHandler.cpp @@ -175,7 +175,8 @@ void StrComHandler::setDownloadImageName(std::string filename) { void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; } -ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) { +ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname, + startracker::FirmwareTarget target) { { MutexGuard mg(lock); if (state != InternalState::SLEEPING) { @@ -192,8 +193,13 @@ ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) { if (not std::filesystem::exists(flashWrite.fullname)) { return FILE_NOT_EXISTS; } - flashWrite.firstRegion = static_cast(startracker::FirmwareRegions::FIRST); - flashWrite.lastRegion = static_cast(startracker::FirmwareRegions::LAST); + if (target == startracker::FirmwareTarget::MAIN) { + flashWrite.firstRegion = static_cast(startracker::FirmwareRegions::FIRST_MAIN); + flashWrite.lastRegion = static_cast(startracker::FirmwareRegions::LAST_MAIN); + } else if (target == startracker::FirmwareTarget::BACKUP) { + flashWrite.firstRegion = static_cast(startracker::FirmwareRegions::FIRST_BACKUP); + flashWrite.lastRegion = static_cast(startracker::FirmwareRegions::LAST_BACKUP); + } { MutexGuard mg(lock); replyWasReceived = false; @@ -275,7 +281,7 @@ ReturnValue_t StrComHandler::performImageDownload() { file.close(); return result; } - result = checkActionReply(replySize); + result = checkActionReply(replySize, "downloading image"); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { serial::flushRxBuf(serialPort); @@ -348,7 +354,7 @@ ReturnValue_t StrComHandler::performImageUpload() { if (result != returnvalue::OK) { return returnvalue::FAILED; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "sky image upload"); if (result != returnvalue::OK) { return result; } @@ -374,7 +380,7 @@ ReturnValue_t StrComHandler::performImageUpload() { if (result != returnvalue::OK) { return returnvalue::FAILED; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "sky image upload"); if (result != returnvalue::OK) { return result; } @@ -388,8 +394,7 @@ ReturnValue_t StrComHandler::performImageUpload() { ReturnValue_t StrComHandler::performFirmwareUpdate() { using namespace startracker; ReturnValue_t result = returnvalue::OK; - result = unlockAndEraseRegions(static_cast(startracker::FirmwareRegions::FIRST), - static_cast(startracker::FirmwareRegions::LAST)); + result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion); if (result != returnvalue::OK) { return result; } @@ -421,6 +426,8 @@ ReturnValue_t StrComHandler::performFlashWrite() { file.seekg(0, file.end); fileSize = file.tellg(); if (fileSize > FLASH_REGION_SIZE * (flashWrite.lastRegion - flashWrite.firstRegion)) { + sif::debug << "Last region: " << (int)flashWrite.lastRegion + << " first region: " << (int)flashWrite.firstRegion << std::endl; sif::warning << "StrHelper::performFlashWrite: Invalid file" << std::endl; return returnvalue::FAILED; } @@ -445,7 +452,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { if (result != returnvalue::OK) { return result; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "firmware image upload"); if (result != returnvalue::OK) { return result; } @@ -488,7 +495,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { if (result != returnvalue::OK) { return result; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "flash write"); if (result != returnvalue::OK) { return result; } @@ -542,7 +549,7 @@ ReturnValue_t StrComHandler::performFlashRead() { file.close(); return result; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "flash read"); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { serial::flushRxBuf(serialPort); @@ -584,7 +591,7 @@ ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) { return readOneReply(failParameter); } -ReturnValue_t StrComHandler::checkActionReply(size_t replySize) { +ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) { uint8_t type = startracker::getReplyFrameType(replyPtr); if (type != TMTC_ACTIONREPLY) { sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl; @@ -592,7 +599,7 @@ ReturnValue_t StrComHandler::checkActionReply(size_t replySize) { } uint8_t status = startracker::getStatusField(replyPtr); if (status != ArcsecDatalinkLayer::STATUS_OK) { - sif::warning << "StrHelper::checkActionReply: Status failure: " + sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": " << static_cast(status) << std::endl; return STATUS_ERROR; } @@ -744,15 +751,15 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { struct UnlockActionRequest unlockReq; struct EraseActionRequest eraseReq; uint32_t size = 0; - for (uint32_t idx = from; idx <= to; idx++) { + for (uint32_t idx = from; idx < to; idx++) { unlockReq.region = idx; - unlockReq.code = startracker::region_secrets::secret[idx]; + unlockReq.code = startracker::region_secrets::SECRETS[idx]; arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size); result = sendAndRead(size, unlockReq.region); if (result != returnvalue::OK) { return result; } - result = checkActionReply(replyLen); + result = checkActionReply(replyLen, "unlocking region"); if (result != returnvalue::OK) { sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id " << static_cast(unlockReq.region) << std::endl; @@ -761,6 +768,9 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { eraseReq.region = idx; arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size); result = sendAndRead(size, eraseReq.region); + if (result != returnvalue::OK) { + } + result = checkActionReply(replyLen, "erasing region"); if (result != returnvalue::OK) { sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id " << static_cast(eraseReq.region) << std::endl; diff --git a/linux/acs/StrComHandler.h b/linux/acs/StrComHandler.h index 252b2466..0fc11629 100644 --- a/linux/acs/StrComHandler.h +++ b/linux/acs/StrComHandler.h @@ -6,6 +6,7 @@ #include #include "OBSWConfig.h" +#include "mission/acs/str/strHelpers.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" @@ -127,7 +128,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public * @param fullname Full name including absolute path of file containing firmware * update. */ - ReturnValue_t startFirmwareUpdate(std::string fullname); + ReturnValue_t startFirmwareUpdate(std::string fullname, startracker::FirmwareTarget target); /** * @brief Starts the flash read procedure @@ -334,7 +335,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public * * @return returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED */ - ReturnValue_t checkActionReply(size_t replySize); + ReturnValue_t checkActionReply(size_t replySize, const char *context); /** * @brief Checks the position field in a star tracker upload/download reply. diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 7999519c..36fb78b0 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -5,7 +5,11 @@ #include #include +#include + #include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/returnvalues/returnvalue.h" +#include "mission/memory/SdCardMountedIF.h" extern "C" { #include @@ -16,7 +20,6 @@ extern "C" { } #include -#include #include #include "OBSWConfig.h" @@ -27,7 +30,8 @@ std::atomic_bool JCFG_DONE(false); StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, StrComHandler* strHelper, power::Switch_t powerSwitch, - startracker::SdCardConfigPathGetter& cfgPathGetter) + startracker::SdCardConfigPathGetter& cfgPathGetter, + SdCardMountedIF& sdCardIF) : DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), @@ -60,6 +64,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, contrastSet(this), strHelper(strHelper), powerSwitch(powerSwitch), + sdCardIF(sdCardIF), cfgPathGetter(cfgPathGetter) { if (comCookie == nullptr) { sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl; @@ -138,6 +143,9 @@ ReturnValue_t StarTrackerHandler::initialize() { // delay whole satellite boot process. reloadJsonCfgFile(); + // Default firmware target is always initialized from persistent file. + loadTargetFirmwareFromPersistentCfg(); + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (manager == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -165,6 +173,19 @@ ReturnValue_t StarTrackerHandler::initialize() { return returnvalue::OK; } +void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() { + const char* prefix = sdCardIF.getCurrentMountPrefix(); + std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH; + std::ifstream ifile(path); + if (ifile.is_open()) { + std::string targetStr; + std::getline(ifile, targetStr); + if (targetStr == "backup\n") { + firmwareTargetRaw = static_cast(startracker::FirmwareTarget::BACKUP); + } + } +} + bool StarTrackerHandler::reloadJsonCfgFile() { jcfgCountdown.resetTimer(); auto strCfgPath = cfgPathGetter.getCfgPath(); @@ -307,21 +328,11 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu strHelper->setFlashReadFilename(std::string(reinterpret_cast(data), size)); return EXECUTION_FINISHED; } - case (startracker::FIRMWARE_UPDATE): { - result = DeviceHandlerBase::acceptExternalDeviceCommands(); - if (result != returnvalue::OK) { - return result; - } - if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { - return FILE_PATH_TOO_LONG; - } - result = - strHelper->startFirmwareUpdate(std::string(reinterpret_cast(data), size)); - if (result != returnvalue::OK) { - return result; - } - strHelperHandlingSpecialRequest = true; - return EXECUTION_FINISHED; + case (startracker::FIRMWARE_UPDATE_MAIN): { + return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN); + } + case (startracker::FIRMWARE_UPDATE_BACKUP): { + return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP); } default: break; @@ -330,6 +341,23 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu reinitNextSetParam = true; return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size); } +ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size, + startracker::FirmwareTarget target) { + ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands(); + if (result != returnvalue::OK) { + return result; + } + if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { + return FILE_PATH_TOO_LONG; + } + result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast(data), size), + target); + if (result != returnvalue::OK) { + return result; + } + strHelperHandlingSpecialRequest = true; + return EXECUTION_FINISHED; +} void StarTrackerHandler::performOperationHook() { EventMessage event; @@ -569,7 +597,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi return returnvalue::OK; } case (startracker::BOOT): { - prepareBootCommand(); + prepareBootCommand(static_cast(firmwareTargetRaw)); return returnvalue::OK; } case (startracker::REQ_VERSION): { @@ -1659,7 +1687,8 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) { case startracker::UPLOAD_IMAGE: case startracker::DOWNLOAD_IMAGE: case startracker::FLASH_READ: - case startracker::FIRMWARE_UPDATE: { + case startracker::FIRMWARE_UPDATE_BACKUP: + case startracker::FIRMWARE_UPDATE_MAIN: { return DeviceHandlerBase::acceptExternalDeviceCommands(); default: break; @@ -1956,9 +1985,9 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command return result; } -void StarTrackerHandler::prepareBootCommand() { +void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) { uint32_t length = 0; - struct BootActionRequest bootRequest = {BOOT_REGION_ID}; + struct BootActionRequest bootRequest = {static_cast(target)}; arc_pack_boot_action_req(&bootRequest, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; @@ -2389,7 +2418,8 @@ ReturnValue_t StarTrackerHandler::checkProgram() { internalState = InternalState::DONE; } break; - case startracker::Program::FIRMWARE: + case startracker::Program::FIRMWARE_BACKUP: + case startracker::Program::FIRMWARE_MAIN: { if (startupState == StartupState::WAIT_CHECK_PROGRAM) { startupState = StartupState::BOOT_BOOTLOADER; } @@ -2400,9 +2430,10 @@ ReturnValue_t StarTrackerHandler::checkProgram() { internalState = InternalState::FAILED_BOOTLOADER_BOOT; } break; + } default: - sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID" - << std::endl; + sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID " + << (int)versionSet.program.value << std::endl; return INVALID_PROGRAM; } return returnvalue::OK; @@ -2865,12 +2896,13 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_CENTROID: case startracker::REQ_CENTROIDS: case startracker::REQ_CONTRAST: { - if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) { + if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE_MAIN) { return STARTRACKER_NOT_RUNNING_FIRMWARE; } break; } - case startracker::FIRMWARE_UPDATE: + case startracker::FIRMWARE_UPDATE_MAIN: + case startracker::FIRMWARE_UPDATE_BACKUP: case startracker::FLASH_READ: if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) { return STARTRACKER_NOT_RUNNING_BOOTLOADER; @@ -2883,3 +2915,43 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { } ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; } + +ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { + auto firmwareTargetUpdate = [&](bool persistent) { + uint8_t value = 0; + newValues->getElement(&value); + if (value != static_cast(startracker::FirmwareTarget::MAIN) && + value != static_cast(startracker::FirmwareTarget::BACKUP)) { + return HasParametersIF::INVALID_VALUE; + } + parameterWrapper->set(firmwareTargetRaw); + if (persistent) { + if (sdCardIF.isSdCardUsable(std::nullopt)) { + const char* prefix = sdCardIF.getCurrentMountPrefix(); + std::filesystem::path path = + std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH; + std::ofstream of(path, std::ofstream::out | std::ofstream::trunc); + if (value == static_cast(startracker::FirmwareTarget::MAIN)) { + of << "main\n"; + } else { + of << "backup\n"; + } + } else { + sif::warning << "SD card not usable" << std::endl; + return returnvalue::FAILED; + } + }; + return returnvalue::OK; + }; + if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) { + return firmwareTargetUpdate(false); + } + if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) { + return firmwareTargetUpdate(true); + } + return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues, + startAtIndex); +} diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index 91651e04..f6611ed6 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -11,10 +11,7 @@ #include #include -#include "OBSWConfig.h" -#include "devices/powerSwitcherList.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" -#include "fsfw/src/fsfw/serialize/SerializeAdapter.h" #include "fsfw/timemanager/Countdown.h" extern "C" { @@ -45,7 +42,7 @@ class StarTrackerHandler : public DeviceHandlerBase { */ StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, StrComHandler* strHelper, power::Switch_t powerSwitch, - startracker::SdCardConfigPathGetter& cfgPathGetter); + startracker::SdCardConfigPathGetter& cfgPathGetter, SdCardMountedIF& sdCardIF); virtual ~StarTrackerHandler(); ReturnValue_t initialize() override; @@ -61,6 +58,9 @@ class StarTrackerHandler : public DeviceHandlerBase { Submode_t getInitialSubmode() override; + ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, uint16_t startAtIndex) override; + protected: void doStartUp() override; void doShutDown() override; @@ -161,7 +161,8 @@ class StarTrackerHandler : public DeviceHandlerBase { // Ping request will reply ping with this ID (data field) static const uint32_t PING_ID = 0x55; - static const uint32_t BOOT_REGION_ID = 1; + uint8_t firmwareTargetRaw = static_cast(startracker::FirmwareTarget::MAIN); + static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static const uint32_t MUTEX_TIMEOUT = 20; static const uint32_t BOOT_TIMEOUT = 1000; @@ -314,12 +315,14 @@ class StarTrackerHandler : public DeviceHandlerBase { std::set additionalRequestedTm{}; std::set::iterator currentSecondaryTmIter; + SdCardMountedIF& sdCardIF; startracker::SdCardConfigPathGetter& cfgPathGetter; /** * @brief Handles internal state */ void handleInternalState(); + void loadTargetFirmwareFromPersistentCfg(); /** * @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution. @@ -380,7 +383,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * @brief Fills command buffer with data to boot image (works only when star tracker is * in bootloader mode). */ - void prepareBootCommand(); + void prepareBootCommand(startracker::FirmwareTarget target); /** * @brief Fills command buffer with command to get the checksum of a flash part @@ -550,6 +553,9 @@ class StarTrackerHandler : public DeviceHandlerBase { void bootBootloader(); bool reloadJsonCfgFile(); ReturnValue_t acceptExternalDeviceCommands() override; + + ReturnValue_t handleFirmwareUpdateCommand(const uint8_t* data, size_t size, + startracker::FirmwareTarget target); }; #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */ diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 9f284725..e73a58af 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -14,6 +14,12 @@ namespace startracker { static const Submode_t SUBMODE_BOOTLOADER = 1; static const Submode_t SUBMODE_FIRMWARE = 2; +enum class FirmwareTarget : uint8_t { MAIN = 1, BACKUP = 10 }; + +static constexpr char FW_TARGET_CFG_PATH[] = "startracker/fw-target.txt"; + +enum ParamId : uint32_t { FIRMWARE_TARGET = 1, FIRMWARE_TARGET_PERSISTENT = 2 }; + class SdCardConfigPathGetter { public: virtual ~SdCardConfigPathGetter() = default; @@ -373,7 +379,7 @@ static const DeviceCommandId_t REQ_DEBUG_CAMERA = 80; static const DeviceCommandId_t LOGLEVEL = 81; static const DeviceCommandId_t LOGSUBSCRIPTION = 82; static const DeviceCommandId_t DEBUG_CAMERA = 83; -static const DeviceCommandId_t FIRMWARE_UPDATE = 84; +static const DeviceCommandId_t FIRMWARE_UPDATE_MAIN = 84; static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85; static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86; static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87; @@ -388,6 +394,7 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95; static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96; static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97; static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100; +static const DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -489,7 +496,8 @@ static constexpr uint8_t MATCHED_CENTROIDS = 40; namespace Program { static const uint8_t BOOTLOADER = 1; -static const uint8_t FIRMWARE = 2; +static const uint8_t FIRMWARE_MAIN = 2; +static const uint8_t FIRMWARE_BACKUP = 3; } // namespace Program namespace region_secrets { @@ -509,7 +517,7 @@ static const uint32_t REGION_12_SECRET = 0x42fedef6; static const uint32_t REGION_13_SECRET = 0xe53cf10d; static const uint32_t REGION_14_SECRET = 0xe862b70b; static const uint32_t REGION_15_SECRET = 0x79b537ca; -static const uint32_t secret[16]{ +static const uint32_t SECRETS[16]{ REGION_0_SECRET, REGION_1_SECRET, REGION_2_SECRET, REGION_3_SECRET, REGION_4_SECRET, REGION_5_SECRET, REGION_6_SECRET, REGION_7_SECRET, REGION_8_SECRET, REGION_9_SECRET, REGION_10_SECRET, REGION_11_SECRET, @@ -538,7 +546,12 @@ enum class FlashSections : uint8_t { }; // Flash region IDs of firmware partition -enum class FirmwareRegions : uint32_t { FIRST = 1, LAST = 8 }; +enum class FirmwareRegions : uint32_t { + FIRST_MAIN = 1, + LAST_MAIN = 8, + FIRST_BACKUP = 10, + LAST_BACKUP = 16 +}; static const uint32_t FLASH_REGION_SIZE = 0x20000; diff --git a/tmtc b/tmtc index a5ebac62..85fc106a 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a5ebac626682e86b45f991c919a3ce9a9b7fcfcc +Subproject commit 85fc106a9b0e6d4256e6243200b9c0a3b910f67c From 9a1d91c26133218a6504bc5064b5b74c2264ffba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Feb 2024 17:16:56 +0100 Subject: [PATCH 19/40] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index baccb4cb..e3e311f8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,10 @@ will consitute of a breaking change warranting a new major release: - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. +## Added + +- Updated STR handler to unlock and allow using the secondary firmware slot. + # [v7.6.1] 2024-02-05 ## Changed From 2d9236824009fc3a721c3b9c9e98a2be160f96b0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Feb 2024 17:23:02 +0100 Subject: [PATCH 20/40] small bugfix --- mission/acs/str/StarTrackerHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 36fb78b0..0858b48d 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -180,7 +180,7 @@ void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() { if (ifile.is_open()) { std::string targetStr; std::getline(ifile, targetStr); - if (targetStr == "backup\n") { + if (targetStr == "backup") { firmwareTargetRaw = static_cast(startracker::FirmwareTarget::BACKUP); } } From e445f8942aa50b6f513551435dc916c1721c2714 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 22 Feb 2024 15:04:30 +0100 Subject: [PATCH 21/40] limit rotation --- mission/controller/acs/Guidance.cpp | 54 ++++++++++++++++++++++++++++- mission/controller/acs/Guidance.h | 3 ++ 2 files changed, 56 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 2972dfed..079b627d 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -180,6 +180,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); + limitReferenceRotation(xAxisIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } @@ -232,6 +233,54 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } +void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) { + if ((VectorOperations::norm(quatIXprev, 4) == 0) or + (VectorOperations::norm(xAxisIXprev, 3) == 0)) { + return; + } + + // check required rotation and return if below limit + double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatIXprev, quatXprevI); + QuaternionOperations::multiply(quatXprevI, quatIX, quatXprevX); + double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * + acsParameters->onBoardParams.sampleTime; + if (std::acos(quatXprevX[3]) < phiMax) { + return; + } + + // x-axis always needs full rotation + double phiX = 0, phiXvec[3] = {0, 0, 0}; + phiX = std::acos(VectorOperations::dot(xAxisIXprev, xAxisIX)); + VectorOperations::cross(xAxisIXprev, xAxisIX, phiXvec); + VectorOperations::normalize(phiXvec, phiXvec, 3); + + double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(phiXvec, std::cos(phiX / 2.), phiXvec, 3); + std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec)); + quatXprevXtilde[3] = cos(phiX / 2.); + QuaternionOperations::multiply(quatIXprev, quatXprevXtilde, quatIXtilde); + + // use the residual rotation up to the maximum + double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0}; + QuaternionOperations::multiply(quatXI, quatIXtilde, quatXXtilde); + + double phiResidual = 0, phiResidualVec[3] = {0, 0, 0}; + phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX)); + std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec)); + VectorOperations::normalize(phiResidualVec, phiResidualVec, 3); + + double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(phiResidualVec, std::cos(phiResidual / 2.), phiResidualVec, + 3); + std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec)); + quatXhatXTilde[3] = cos(phiResidual / 2.); + + // calculate final quaternion + QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat); + QuaternionOperations::multiply(quatIXtilde, quatXTildeXhat, quatIX); +} + void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { @@ -298,7 +347,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, return acsctrl::MULTIPLE_RW_UNAVAILABLE; } -void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); } +void Guidance::resetValues() { + std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); + std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev)); +} void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) { std::error_code e; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 5284ba01..a914ecfe 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -35,6 +35,8 @@ class Guidance { void targetRotationRate(const double timeDelta, double quatInertialTarget[4], double *targetSatRotRate); + void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]); + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); @@ -52,6 +54,7 @@ class Guidance { bool strBlindAvoidFlag = false; double quatIXprev[4] = {0, 0, 0, 0}; + double xAxisIXprev[3] = {0, 0, 0}; static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; From 5e6b0f5ea295d5b67665dc0d7f44f59f7aaf2e68 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 22 Feb 2024 15:08:27 +0100 Subject: [PATCH 22/40] changelog --- CHANGELOG.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 657043fc..ad62cbb7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,7 +31,9 @@ will consitute of a breaking change warranting a new major release: ## Changed - `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw` -- Changed pointing strategy for target groundstation mode to prevent blinding of the STR +- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This + also limits the rotation for the reference target quaternion to prevent spikes in required + rotation rates. # [v7.6.1] 2024-02-05 From 58a8c5869c694527188406ec1cc9a6f825145ef2 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 22 Feb 2024 16:59:26 +0100 Subject: [PATCH 23/40] some fixes and debug stuff --- mission/controller/acs/Guidance.cpp | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 079b627d..c70c37e3 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -102,9 +102,12 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame - double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; - CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); - CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute); + double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364}, + posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924}, + groundStationDirI[3] = {0, 0, 0}; + // CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + // CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, + // &timeAbsolute); VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction @@ -119,32 +122,34 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, VectorOperations::mulScalar(earthDirI, -1, earthDirI, 3); // sun avoidance calculations - double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}; + double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, sunDirI), sunPerpendicularX, 3); VectorOperations::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3); VectorOperations::normalize(sunFloorYZ, sunFloorYZ, 3); + VectorOperations::mulScalar(sunFloorYZ, -1, zAxisSun, 3); double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, strVecSunZ[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], strVecSunX, 3); - VectorOperations::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2], + VectorOperations::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2], strVecSunZ, 3); VectorOperations::add(strVecSunX, strVecSunZ, strVecSun, 3); VectorOperations::normalize(strVecSun, strVecSun, 3); sunWeight = VectorOperations::dot(strVecSun, sunDirI); // earth avoidance calculations - double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}; + double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, earthDirI), earthPerpendicularX, 3); VectorOperations::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3); VectorOperations::normalize(earthFloorYZ, earthFloorYZ, 3); + VectorOperations::mulScalar(earthFloorYZ, -1, zAxisEarth, 3); double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, strVecEarthZ[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], strVecEarthX, 3); - VectorOperations::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2], + VectorOperations::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2], strVecEarthZ, 3); VectorOperations::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); VectorOperations::normalize(strVecEarth, strVecEarth, 3); @@ -162,9 +167,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, earthWeight *= normFactor; // calculate z-axis for str blinding avoidance - double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0}; - VectorOperations::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3); - VectorOperations::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3); + double zAxisIX[3] = {0, 0, 0}; + VectorOperations::mulScalar(zAxisSun, sunWeight, zAxisSun, 3); + VectorOperations::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3); VectorOperations::add(zAxisSun, zAxisEarth, zAxisIX, 3); VectorOperations::mulScalar(zAxisIX, -1, zAxisIX, 3); VectorOperations::normalize(zAxisIX, zAxisIX, 3); @@ -249,6 +254,8 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) return; } + sif::debug << "Reduced Quaternion Rotation Required" << std::endl; + // x-axis always needs full rotation double phiX = 0, phiXvec[3] = {0, 0, 0}; phiX = std::acos(VectorOperations::dot(xAxisIXprev, xAxisIX)); From 75654277b238816e008dbc4569287d4e5468594c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:05:10 +0100 Subject: [PATCH 24/40] pain --- mission/controller/acs/AcsParameters.h | 2 +- mission/controller/acs/Guidance.cpp | 35 +++++++++++++++++--------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 6bc20594..0e6eeec6 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -915,7 +915,7 @@ class AcsParameters : public HasParametersIF { struct StrParameters { double exclusionAngle = 20. * DEG2RAD; - double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame + double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf } strParameters; struct GpsParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index c70c37e3..f11198eb 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -102,8 +102,8 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame - double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364}, - posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924}, + double posSatI[3] = {3808618.68633079, -1163140.41530084, 5612341.3560814}, + posGroundStationI[3] = {3737093.20736689, -1967773.50625331, 4763980.76688758}, groundStationDirI[3] = {0, 0, 0}; // CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); // CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, @@ -187,6 +187,8 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, limitReferenceRotation(xAxisIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); + + std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev)); } void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, @@ -239,6 +241,10 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub } void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) { + sif::debug << "xAxisIX = [" << xAxisIX[0] << " " << xAxisIX[1] << " " << xAxisIX[2] << "]" + << std::endl; + sif::debug << "quatIX = [" << quatIX[3] << " " << quatIX[0] << " " << quatIX[1] << " " + << quatIX[2] << "]" << std::endl; if ((VectorOperations::norm(quatIXprev, 4) == 0) or (VectorOperations::norm(xAxisIXprev, 3) == 0)) { return; @@ -247,10 +253,11 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) // check required rotation and return if below limit double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0}; QuaternionOperations::inverse(quatIXprev, quatXprevI); - QuaternionOperations::multiply(quatXprevI, quatIX, quatXprevX); - double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * - acsParameters->onBoardParams.sampleTime; - if (std::acos(quatXprevX[3]) < phiMax) { + QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX); + QuaternionOperations::normalize(quatXprevX); + double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * 1; + // acsParameters->onBoardParams.sampleTime; + if (2 * std::acos(quatXprevX[3]) < phiMax) { return; } @@ -263,14 +270,16 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) VectorOperations::normalize(phiXvec, phiXvec, 3); double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0}; - VectorOperations::mulScalar(phiXvec, std::cos(phiX / 2.), phiXvec, 3); + VectorOperations::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3); std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec)); quatXprevXtilde[3] = cos(phiX / 2.); - QuaternionOperations::multiply(quatIXprev, quatXprevXtilde, quatIXtilde); + QuaternionOperations::normalize(quatXprevXtilde); + QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde); // use the residual rotation up to the maximum double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0}; - QuaternionOperations::multiply(quatXI, quatIXtilde, quatXXtilde); + QuaternionOperations::inverse(quatIX, quatXI); + QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde); double phiResidual = 0, phiResidualVec[3] = {0, 0, 0}; phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX)); @@ -278,14 +287,16 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) VectorOperations::normalize(phiResidualVec, phiResidualVec, 3); double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0}; - VectorOperations::mulScalar(phiResidualVec, std::cos(phiResidual / 2.), phiResidualVec, + VectorOperations::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec, 3); std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec)); - quatXhatXTilde[3] = cos(phiResidual / 2.); + quatXhatXTilde[3] = std::cos(phiResidual / 2.); + QuaternionOperations::normalize(quatXhatXTilde); // calculate final quaternion QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat); - QuaternionOperations::multiply(quatIXtilde, quatXTildeXhat, quatIX); + QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX); + QuaternionOperations::normalize(quatIX); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], From 2f43f58792ccb8ac1e734d4c1da83a0092ecf666 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:10:52 +0100 Subject: [PATCH 25/40] cleanup --- fsfw | 2 +- mission/controller/acs/Guidance.cpp | 19 +++++-------------- 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/fsfw b/fsfw index 90f3f8ef..7d4e7784 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 90f3f8ef1f67dc6d07c242b68bc59c805bb407dd +Subproject commit 7d4e77843b64e812e537e2abd1213c632b6eeb48 diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f11198eb..4f7727f2 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -102,12 +102,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame - double posSatI[3] = {3808618.68633079, -1163140.41530084, 5612341.3560814}, - posGroundStationI[3] = {3737093.20736689, -1967773.50625331, 4763980.76688758}, - groundStationDirI[3] = {0, 0, 0}; - // CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); - // CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, - // &timeAbsolute); + double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute); VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction @@ -241,10 +238,6 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub } void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) { - sif::debug << "xAxisIX = [" << xAxisIX[0] << " " << xAxisIX[1] << " " << xAxisIX[2] << "]" - << std::endl; - sif::debug << "quatIX = [" << quatIX[3] << " " << quatIX[0] << " " << quatIX[1] << " " - << quatIX[2] << "]" << std::endl; if ((VectorOperations::norm(quatIXprev, 4) == 0) or (VectorOperations::norm(xAxisIXprev, 3) == 0)) { return; @@ -255,14 +248,12 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) QuaternionOperations::inverse(quatIXprev, quatXprevI); QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX); QuaternionOperations::normalize(quatXprevX); - double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * 1; - // acsParameters->onBoardParams.sampleTime; + double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * + acsParameters->onBoardParams.sampleTime; if (2 * std::acos(quatXprevX[3]) < phiMax) { return; } - sif::debug << "Reduced Quaternion Rotation Required" << std::endl; - // x-axis always needs full rotation double phiX = 0, phiXvec[3] = {0, 0, 0}; phiX = std::acos(VectorOperations::dot(xAxisIXprev, xAxisIX)); From 4d5ac727a870f9c0d480be03e0093ab57275b15e Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:22:03 +0100 Subject: [PATCH 26/40] changed sus filter param --- mission/controller/acs/AcsParameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 0e6eeec6..bf2749bd 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -776,7 +776,7 @@ class AcsParameters : public HasParametersIF { 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; float susBrightnessThreshold = 0.7; - float susVectorFilterWeight = .85; + float susVectorFilterWeight = .95; float susRateFilterWeight = .99; } susHandlingParameters; From ff3bf4e29153dc5779b2bd79103aaced7cb56c9d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:22:59 +0100 Subject: [PATCH 27/40] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ad62cbb7..9cb0cdb5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,7 @@ will consitute of a breaking change warranting a new major release: - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid. - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. +- Updated QUEST and Sun Vector Params to new values. ## Changed From 2e08f3b393a24b496084dc55b61f016a356f2f28 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:27:01 +0100 Subject: [PATCH 28/40] not that we need this right now --- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index efa74568..647e2141 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1079,7 +1079,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); if (not(VectorOperations::isFinite(propagatedQuaternion, 4)) || - not(MatrixOperations::isFinite(initialQuaternion, 6, 6))) { + not(MatrixOperations::isFinite(*initialCovarianceMatrix, 6, 6))) { updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); return MEKF_NOT_FINITE; } From a8172e641f7395c8ef2408732affc1406a809cfe Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:34:03 +0100 Subject: [PATCH 29/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 7d4e7784..b2576d34 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7d4e77843b64e812e537e2abd1213c632b6eeb48 +Subproject commit b2576d34225f2cdb4cf89788bbd254d470ad6c20 From 688943cacbed87adb395b761db47bf131ad66ce5 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 11:35:49 +0100 Subject: [PATCH 30/40] thanks for automatically not updating the references eclipse --- mission/controller/acs/Igrf13Model.h | 2 +- mission/controller/acs/SensorProcessing.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 774d6c58..9a7feb34 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,11 +16,11 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ +#include #include #include #include #include -#include #include diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 35d3ae6d..e5efba71 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -4,11 +4,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include #include From 2965c63bf677e660ef1b9d7c0dd5fec8f89a57a1 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 13:16:17 +0100 Subject: [PATCH 31/40] fixed changelog --- CHANGELOG.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2d32f080..63e8e4b6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,9 +32,6 @@ will consitute of a breaking change warranting a new major release: - `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes - QUEST and STR rates are now allowed per default - Changed PTG Strat priorities to favor STR before MEKF. - -## Changed - - Increased message queue depth and maximum number of handled messages per cycle for `PusServiceBase` based classes (especially PUS scheduler). From f4d1dbb5df2b33e567e0a4a10f1f0b67bcacbe8c Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 13:57:57 +0100 Subject: [PATCH 32/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index cd2cd61b..0d4a862c 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit cd2cd61ba575603f53178d649a3fe344ddbcaa55 +Subproject commit 0d4a862c1af78ee5568b3268afc526be70fa055b From 4c589e0c1b5f41cbc9b41e6a188a334e5a03a013 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 13:58:47 +0100 Subject: [PATCH 33/40] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 63e8e4b6..c19464bf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Bumped `eive-fsfw` + ## Fixed - PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report From ab71014f672850e0b53a6bc81457b0b729c8d5fc Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 14:06:23 +0100 Subject: [PATCH 34/40] bumped tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 85fc106a..588d7a80 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 85fc106a9b0e6d4256e6243200b9c0a3b910f67c +Subproject commit 588d7a8079194c6c51d6ecafd4c940cb1bf0554c From 386430b9f2b0b47ffd86090d5405772fc2cc89bc Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 14:06:59 +0100 Subject: [PATCH 35/40] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 150a79e0..6acf41d0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Bumped `eive-tmtc` to - Bumped `eive-fsfw` ## Fixed From 9e1e286b97915da106c76235792ebf7feabab59e Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 16:27:14 +0100 Subject: [PATCH 36/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index b2576d34..ac28b7e0 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit b2576d34225f2cdb4cf89788bbd254d470ad6c20 +Subproject commit ac28b7e00d51876ad884dc07a4f81f4d6a68b103 From c434882e3727f8497c14cd246884451eaf629205 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 10:54:26 +0100 Subject: [PATCH 37/40] code review --- mission/acs/str/StarTrackerHandler.cpp | 10 +++++----- mission/genericFactory.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 0858b48d..2a07def5 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -7,6 +7,7 @@ #include +#include "fsfw/filesystem/HasFileSystemIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/returnvalues/returnvalue.h" #include "mission/memory/SdCardMountedIF.h" @@ -177,7 +178,7 @@ void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() { const char* prefix = sdCardIF.getCurrentMountPrefix(); std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH; std::ifstream ifile(path); - if (ifile.is_open()) { + if (ifile.is_open() and !ifile.bad()) { std::string targetStr; std::getline(ifile, targetStr); if (targetStr == "backup") { @@ -2896,7 +2897,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_CENTROID: case startracker::REQ_CENTROIDS: case startracker::REQ_CONTRAST: { - if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE_MAIN) { + if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) { return STARTRACKER_NOT_RUNNING_FIRMWARE; } break; @@ -2904,7 +2905,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::FIRMWARE_UPDATE_MAIN: case startracker::FIRMWARE_UPDATE_BACKUP: case startracker::FLASH_READ: - if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) { + if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) { return STARTRACKER_NOT_RUNNING_BOOTLOADER; } break; @@ -2940,8 +2941,7 @@ ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueI of << "backup\n"; } } else { - sif::warning << "SD card not usable" << std::endl; - return returnvalue::FAILED; + return HasFileSystemIF::FILESYSTEM_INACTIVE; } }; return returnvalue::OK; diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index daaf59cc..c5a69b26 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -253,7 +253,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun auto psbParamsService5 = PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5); - psbParamsService5.requestQueueDepth = 50; + psbParamsService5.requestQueueDepth = 50; psbParamsService5.maxPacketsPerCycle = 50; new Service5EventReporting(psbParamsService5, 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, From 69f4b6de06096f299458f17e8b219dbd7019fd57 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 10:55:53 +0100 Subject: [PATCH 38/40] I LOVE STATIC_CAST. I LOVE TYPING A LOT --- mission/acs/str/StarTrackerHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 2a07def5..a88014ab 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -2434,7 +2434,7 @@ ReturnValue_t StarTrackerHandler::checkProgram() { } default: sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID " - << (int)versionSet.program.value << std::endl; + << static_cast(versionSet.program.value) << std::endl; return INVALID_PROGRAM; } return returnvalue::OK; From 2ac9f972da6c113e1f2ca4e6a7be7316269262fb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 10:58:23 +0100 Subject: [PATCH 39/40] remove debug printout --- linux/acs/StrComHandler.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/linux/acs/StrComHandler.cpp b/linux/acs/StrComHandler.cpp index a02d47d1..9c5e045f 100644 --- a/linux/acs/StrComHandler.cpp +++ b/linux/acs/StrComHandler.cpp @@ -426,8 +426,6 @@ ReturnValue_t StrComHandler::performFlashWrite() { file.seekg(0, file.end); fileSize = file.tellg(); if (fileSize > FLASH_REGION_SIZE * (flashWrite.lastRegion - flashWrite.firstRegion)) { - sif::debug << "Last region: " << (int)flashWrite.lastRegion - << " first region: " << (int)flashWrite.firstRegion << std::endl; sif::warning << "StrHelper::performFlashWrite: Invalid file" << std::endl; return returnvalue::FAILED; } From 4cf34cb99aa9bc999b5fb82f622c88e302934afd Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 27 Feb 2024 12:51:20 +0100 Subject: [PATCH 40/40] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index ac28b7e0..516357d8 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit ac28b7e00d51876ad884dc07a4f81f4d6a68b103 +Subproject commit 516357d855c07786b492e981230988186376d301