From 6c90777e4b9bd39858b61c4da57a8215079aae70 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 Jan 2024 11:32:37 +0100 Subject: [PATCH 01/19] imagine being a newspace company and not even knowing what a mathematically positive sense of rotation is --- mission/controller/acs/AcsParameters.cpp | 8 ++++---- mission/controller/acs/AcsParameters.h | 24 ++++++++++++------------ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a6ce6c5d..4731b6ee 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -333,16 +333,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(rwMatrices.pseudoInverse); break; case 0x2: - parameterWrapper->setMatrix(rwMatrices.without1); + parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1); break; case 0x3: - parameterWrapper->setMatrix(rwMatrices.without2); + parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2); break; case 0x4: - parameterWrapper->setMatrix(rwMatrices.without3); + parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3); break; case 0x5: - parameterWrapper->setMatrix(rwMatrices.without4); + parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4); break; case 0x6: parameterWrapper->setVector(rwMatrices.nullspaceVector); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f11a673b..122a932b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -812,19 +812,19 @@ class AcsParameters : public HasParametersIF { } rwHandlingParameters; struct RwMatrices { - double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, - {0.0000, -0.9205, 0.0000, 0.9205}, - {0.3907, 0.3907, 0.3907, 0.3907}}; + double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000}, + {0.0000, 0.9205, 0.0000, -0.9205}, + {-0.3907, -0.3907, -0.3907, -0.3907}}; double pseudoInverse[4][3] = { - {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; - double without1[4][3] = { - {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}}; - double without2[4][3] = { - {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}}; - double without3[4][3] = { - {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; - double without4[4][3] = { - {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; + {-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}}; + double pseudoInverseWithoutRW1[4][3] = { + {0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}}; + double pseudoInverseWithoutRW2[4][3] = { + {-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}}; + double pseudoInverseWithoutRW3[4][3] = { + {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2798}, {0, 0, 0}, {-0.5432, 0.5432, 1.2798}}; + double pseudoInverseWithoutRW4[4][3] = { + {-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}}; double nullspaceVector[4] = {-1, 1, -1, 1}; } rwMatrices; From edf2f889c1b0f5ab736e270379616fc18392d87a Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 Jan 2024 13:15:10 +0100 Subject: [PATCH 02/19] forgot this one --- 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 122a932b..0ab1105d 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -822,7 +822,7 @@ class AcsParameters : public HasParametersIF { double pseudoInverseWithoutRW2[4][3] = { {-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}}; double pseudoInverseWithoutRW3[4][3] = { - {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2798}, {0, 0, 0}, {-0.5432, 0.5432, 1.2798}}; + {-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}}; double pseudoInverseWithoutRW4[4][3] = { {-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}}; double nullspaceVector[4] = {-1, 1, -1, 1}; From 3892276a5c7fd7f4ab62f9921ab058208f7e1ef5 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 Jan 2024 13:19:59 +0100 Subject: [PATCH 03/19] reset guidance on mode change --- mission/controller/AcsController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8e7ae8d4..36d05fb2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -885,6 +885,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { + guidance.resetValues(); return ExtendedControllerBase::modeChanged(mode, submode); } From ff5e1cd76b9c3794d0b5ef6ce8dd75c6225d275c Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 Jan 2024 13:25:11 +0100 Subject: [PATCH 04/19] reset guidance if lost --- mission/controller/AcsController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 36d05fb2..3f44f327 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -412,6 +412,7 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); ptgCtrlLostCounter = 0; } + guidance.resetValues(); updateCtrlValData(ptgCtrlStrat); updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16); commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], From 42b94ab581f2b32b7339512347f5b563f8b14455 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 10:50:11 +0100 Subject: [PATCH 05/19] we can only use the nullspace if all rws are available --- mission/controller/acs/control/PtgCtrl.cpp | 8 ++++++-- mission/controller/acs/control/PtgCtrl.h | 3 ++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index fd5b9f29..8c3a7e7b 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -39,7 +39,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]}; double cInt = 2 * om * zeta; - double kInt = 2 * pow(om, 2); + double kInt = 2 * om * om; double qErrorLaw[3] = {0, 0, 0}; @@ -111,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, +void PtgCtrl::ptgNullspace(const bool allRwAvabilable, + AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs) { + if (not allRwAvabilable) { + return; + } // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), static_cast(speedRw2), static_cast(speedRw3)}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index a07849ec..66402b0d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -33,7 +33,8 @@ class PtgCtrl { void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + void ptgNullspace(const bool allRwAvabilable, + AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); From 10841a01b77a87259410fc9d125a9f90cbe05aae Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 10:59:15 +0100 Subject: [PATCH 06/19] class id is defined here now --- mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 83a74552..78725755 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -8,6 +8,8 @@ namespace acsctrl { +static const uint8_t IF_ACS_CTRL_ID = CLASS_ID::ACS_CTRL; + enum SetIds : uint32_t { MGM_SENSOR_DATA, MGM_PROCESSED_DATA, From 7daeb9a148e08af0f4346f46e0dbeaa10ef0e654 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 10:59:57 +0100 Subject: [PATCH 07/19] better rw failure handling and use new classid location --- mission/controller/AcsController.cpp | 20 ++++++++++++-------- mission/controller/AcsController.h | 10 ++++++---- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3f44f327..2310a99b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -447,9 +447,10 @@ void AcsController::performPointingCtrl() { } uint8_t enableAntiStiction = true; + bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - if (result == returnvalue::FAILED) { + if (result == Guidance::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); @@ -457,8 +458,10 @@ void AcsController::performPointingCtrl() { } multipleRwUnavailableCounter++; return; - } else { - multipleRwUnavailableCounter = 0; + } + multipleRwUnavailableCounter = 0; + if (result == Guidance::SINGLE_RW_UNAVAILABLE) { + allRwAvailable = false; } // Variables required for guidance @@ -476,10 +479,11 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, + ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -500,7 +504,7 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, rwTrqNs); @@ -521,7 +525,7 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, + ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, rwTrqNs); @@ -545,7 +549,7 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, rwTrqNs); @@ -568,7 +572,7 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, rwTrqNs); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index aa2b9411..bb4e3c37 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -108,13 +108,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t UPDATE_TLE = 0x3; static const DeviceCommandId_t READ_TLE = 0x4; - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. - static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); + static constexpr ReturnValue_t FILE_DELETION_FAILED = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 0); //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. - static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1); + static constexpr ReturnValue_t WRITE_FILE_FAILED = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 1); //! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. - static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2); + static constexpr ReturnValue_t READ_FILE_FAILED = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 2); ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; From c1e7ac7e9a6a44dcc0cde99a4dfee28eae49d1bc Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 11:12:28 +0100 Subject: [PATCH 08/19] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index e64e8b27..5879eb7f 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa +Subproject commit 5879eb7f7b83205c64980580c16d3aa78d6ea073 From 291c9ea99ba8763924c2be7eb8c374dea744812d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 11:16:59 +0100 Subject: [PATCH 09/19] calculate the rotation rate in normal way --- mission/controller/acs/Guidance.cpp | 77 +++++++++-------------------- mission/controller/acs/Guidance.h | 25 ++++++++-- 2 files changed, 44 insertions(+), 58 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 10d1393c..9ac7b5e9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -1,15 +1,5 @@ #include "Guidance.h" -#include -#include -#include -#include -#include - -#include -#include -#include - Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} @@ -455,44 +445,16 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do } void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, - double quatInertialTarget[4], double *refSatRate) { - //------------------------------------------------------------------------------------- - // Calculation of target rotation rate - //------------------------------------------------------------------------------------- - if (VectorOperations::norm(savedQuaternion, 4) == 0) { - std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + double quatIX[4], double *refSatRate) { + if (VectorOperations::norm(quatIXprev, 4) == 0) { + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } - if (timeDelta < timeElapsedMax and timeDelta != 0.0) { - double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(quatInertialTarget, q); - QuaternionOperations::inverse(savedQuaternion, qS); - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(q, qS, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1. / timeDelta, qDiff, 4); - - double tgtQuatVec[3] = {q[0], q[1], q[2]}; - double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(tgtQuatVec, qDiffVec, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); - omegaRefSaved[0] = omegaRefNew[0]; - omegaRefSaved[1] = omegaRefNew[1]; - omegaRefSaved[2] = omegaRefNew[2]; + if (not timeDelta != 0.0) { + QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); } else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + std::memcpy(refSatRate, ZERO_VEC3, sizeof(refSatRate)); } - - std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, @@ -506,22 +468,27 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { - std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); - return returnvalue::OK; + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, + 12 * sizeof(double)); + return SINGLE_RW_UNAVAILABLE; } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { - std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); - return returnvalue::OK; + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, + 12 * sizeof(double)); + return SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { - std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); - return returnvalue::OK; + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, + 12 * sizeof(double)); + return SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { - std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); - return returnvalue::OK; - } else { - return returnvalue::FAILED; + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, + 12 * sizeof(double)); + return SINGLE_RW_UNAVAILABLE; } + return MULTIPLE_RW_UNAVAILABLE; } +void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); } + void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 1cf5b8a9..98f77766 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,9 +1,18 @@ #ifndef GUIDANCE_H_ #define GUIDANCE_H_ +#include +#include +#include +#include +#include +#include #include -#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include +#include +#include + #include "AcsParameters.h" #include "SensorValues.h" @@ -14,6 +23,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); + void resetValues(); // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station @@ -56,12 +66,21 @@ class Guidance { // reation wheel maybe can be done in "commanding.h" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + //! [EXPORT] : [COMMENT] A single RW has failed. + static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3); + //! [EXPORT] : [COMMENT] Multiple RWs have failed. + static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4); + private: const AcsParameters *acsParameters; + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[3] = {0, 0, 0, 0}; + bool strBlindAvoidFlag = false; - double savedQuaternion[4] = {0, 0, 0, 0}; - double omegaRefSaved[3] = {0, 0, 0}; + double quatIXprev[4] = {0, 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 944dfb6f81218cfd97b85645cec3628b6d9b46d0 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 13:16:27 +0100 Subject: [PATCH 10/19] convert target rot rate into body rf --- mission/controller/acs/Guidance.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 9ac7b5e9..d7e8fe26 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -421,7 +421,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do errorAngle = QuaternionOperations::getAngle(errorQuat, true); // Calculate error satellite rotational rate - // First combine the target and reference satellite rotational rates + // Convert target rotational rate into body RF + double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0}; + QuaternionOperations::inverse(errorQuat, errorQuatInv); + QuaternionOperations::multiplyVector(errorQuat, targetSatRotRate, targetSatRotRateB); + // Combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then subtract the combined required satellite rotational rates from the actual rate From 588612875d67a78eb780aac76f56ec322dae3420 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 13:36:51 +0100 Subject: [PATCH 11/19] this is not not funny --- mission/controller/acs/Guidance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index d7e8fe26..185158d5 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -453,10 +453,10 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time if (VectorOperations::norm(quatIXprev, 4) == 0) { std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } - if (not timeDelta != 0.0) { + if (timeDelta != 0.0) { QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); } else { - std::memcpy(refSatRate, ZERO_VEC3, sizeof(refSatRate)); + std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); } std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } From 2f296c3c8c444c227a89d1bf860aaec2afacac80 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 13:37:30 +0100 Subject: [PATCH 12/19] whoopsies --- mission/controller/acs/Guidance.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 98f77766..f6319c7b 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include #include @@ -13,9 +15,6 @@ #include #include -#include "AcsParameters.h" -#include "SensorValues.h" - class Guidance { public: Guidance(AcsParameters *acsParameters_); @@ -77,7 +76,7 @@ class Guidance { const AcsParameters *acsParameters; static constexpr double ZERO_VEC3[3] = {0, 0, 0}; - static constexpr double ZERO_VEC4[3] = {0, 0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; bool strBlindAvoidFlag = false; double quatIXprev[4] = {0, 0, 0, 0}; From ec3523e5adc5b455bf2875f75d5f88ee302134df Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 Jan 2024 15:36:01 +0100 Subject: [PATCH 13/19] cleanup --- mission/controller/acs/util/MathOperations.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index dd4c3f57..023e9379 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -4,15 +4,12 @@ #include #include #include -#include -#include -#include +#include #include +#include #include -#include "fsfw/serviceinterface.h" - template class MathOperations { public: @@ -46,7 +43,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 ec2b026103e14dd334c6d269f7cdf4f3afaa3a7b Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 27 Jan 2024 12:50:58 +0100 Subject: [PATCH 14/19] fix? --- mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 78725755..7de3e36e 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -1,6 +1,7 @@ #ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ #define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ +#include #include #include From f7c997980c01c51e46f4185983e3e4fdb0f2d620 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 11:08:47 +0100 Subject: [PATCH 15/19] this makes more sense --- mission/controller/AcsController.cpp | 10 +++++----- mission/controller/AcsController.h | 10 ---------- mission/controller/acs/Guidance.cpp | 10 +++++----- mission/controller/acs/Guidance.h | 7 ------- .../controllerdefinitions/AcsCtrlDefinitions.h | 14 ++++++++++++++ 5 files changed, 24 insertions(+), 27 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2310a99b..22eb7a41 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -450,7 +450,7 @@ void AcsController::performPointingCtrl() { bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - if (result == Guidance::MULTIPLE_RW_UNAVAILABLE) { + if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); @@ -460,7 +460,7 @@ void AcsController::performPointingCtrl() { return; } multipleRwUnavailableCounter = 0; - if (result == Guidance::SINGLE_RW_UNAVAILABLE) { + if (result == acsctrl::SINGLE_RW_UNAVAILABLE) { allRwAvailable = false; } @@ -1087,7 +1087,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { tleFile << "\n"; tleFile.write(reinterpret_cast(tle + 69), 69); } else { - return WRITE_FILE_FAILED; + return acsctrl::WRITE_FILE_FAILED; } tleFile.close(); return returnvalue::OK; @@ -1111,12 +1111,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { std::memcpy(line2, tleLine2.c_str(), 69); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); - return READ_FILE_FAILED; + return acsctrl::READ_FILE_FAILED; } tleFile.close(); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); - return READ_FILE_FAILED; + return acsctrl::READ_FILE_FAILED; } return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index bb4e3c37..928926fb 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -108,16 +108,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t UPDATE_TLE = 0x3; static const DeviceCommandId_t READ_TLE = 0x4; - //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. - static constexpr ReturnValue_t FILE_DELETION_FAILED = - returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 0); - //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. - static constexpr ReturnValue_t WRITE_FILE_FAILED = - returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 1); - //! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. - static constexpr ReturnValue_t READ_FILE_FAILED = - returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 2); - ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 185158d5..3b5d325c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -474,21 +474,21 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); - return SINGLE_RW_UNAVAILABLE; + return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); - return SINGLE_RW_UNAVAILABLE; + return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); - return SINGLE_RW_UNAVAILABLE; + return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, 12 * sizeof(double)); - return SINGLE_RW_UNAVAILABLE; + return acsctrl::SINGLE_RW_UNAVAILABLE; } - return MULTIPLE_RW_UNAVAILABLE; + return acsctrl::MULTIPLE_RW_UNAVAILABLE; } void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index f6319c7b..bdf6d7fb 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -65,13 +65,6 @@ class Guidance { // reation wheel maybe can be done in "commanding.h" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); - //! [EXPORT] : [COMMENT] A single RW has failed. - static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = - returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3); - //! [EXPORT] : [COMMENT] Multiple RWs have failed. - static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = - returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4); - private: const AcsParameters *acsParameters; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 7de3e36e..a9e527f4 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -10,6 +10,20 @@ namespace acsctrl { static const uint8_t IF_ACS_CTRL_ID = CLASS_ID::ACS_CTRL; +//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. +static constexpr ReturnValue_t FILE_DELETION_FAILED = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 0); +//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. +static constexpr ReturnValue_t WRITE_FILE_FAILED = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 1); +//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. +static constexpr ReturnValue_t READ_FILE_FAILED = returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 2); +//! [EXPORT] : [COMMENT] A single RW has failed. +static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3); +//! [EXPORT] : [COMMENT] Multiple RWs have failed. +static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = + returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4); enum SetIds : uint32_t { MGM_SENSOR_DATA, From 64d105cf876ee93c839bba7c692bfa8373d58b17 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 11:11:47 +0100 Subject: [PATCH 16/19] fix --- 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 3b5d325c..12957ab2 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -424,7 +424,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Convert target rotational rate into body RF double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0}; QuaternionOperations::inverse(errorQuat, errorQuatInv); - QuaternionOperations::multiplyVector(errorQuat, targetSatRotRate, targetSatRotRateB); + QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB); // Combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); From 96b74574b0acf6eecaa6b05ff97b18bb26941adf Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 11:12:40 +0100 Subject: [PATCH 17/19] forgot that one --- mission/controller/AcsController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 22eb7a41..41a16919 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -49,7 +49,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: { ReturnValue_t result = guidance.solarArrayDeploymentComplete(); if (result == returnvalue::FAILED) { - return FILE_DELETION_FAILED; + return acsctrl::FILE_DELETION_FAILED; } return HasActionsIF::EXECUTION_FINISHED; } From 8390a0269033ba98ff0dba2e89c722d04db48b46 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 14:15:16 +0100 Subject: [PATCH 18/19] changelog --- CHANGELOG.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8293fdf6..321e5a58 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,9 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Bumped `eive-tmtc` to +- Bumped `eive-fsfw` + ## Added - Added new parameter for MPSoC which allows to skip SUPV commanding. @@ -29,6 +32,13 @@ will consitute of a breaking change warranting a new major release: - If the PCDU handler fails reading data from the IPC store, it will not try to do a deserialization anymore. - All action commands sent by the PLOC SUPV to itself will have no sender now. +- The `AcsController` will reset its stored guidance values on mode change and lost + orientation. +- The nullspace controller will only be used if all RWs are available. +- Calculation of required rotation rate in pointing modes has been fixed to actual + calculation of rotation rate from two quaternions. +- Fixed alignment matrix and pseudo inverses of RWs, to match the wrong definition of + positive rotation. # [v7.5.5] 2024-01-22 From e3ad08d987d35dc901a0a05c7acdbe0455af8c1f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 14:21:43 +0100 Subject: [PATCH 19/19] bumped fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 5879eb7f..b5e7179a 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5879eb7f7b83205c64980580c16d3aa78d6ea073 +Subproject commit b5e7179af1da085b9be598b4897f2664012781af