From 97f7f7c97365a3622645bfa0ccae0a4c95bb6ae4 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 16 Feb 2023 15:40:16 +0100 Subject: [PATCH] RW pseudo inverses now set from RW state. multiple invalids trigger event and disable ptgCtrl --- mission/acsDefs.h | 2 + mission/controller/AcsController.cpp | 6 +- mission/controller/acs/Guidance.cpp | 117 ++++++--------------------- mission/controller/acs/Guidance.h | 2 +- 4 files changed, 31 insertions(+), 96 deletions(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 57ae4730..f68ff24a 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -23,6 +23,8 @@ static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); //!< The system has recovered from a safe rate rotation violation. static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); +//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. +static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 220c477e..352f4167 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -263,7 +263,11 @@ void AcsController::performPointingCtrl() { double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + if (result == returnvalue::FAILED) { + triggerEvent(acs::MULTIPLE_RW_INVALID); + return; + } double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 383a265f..d7db0538 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -610,104 +610,33 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou // under 150 arcsec ?? } -void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { - if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && - sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { - rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; +ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, + double *rwPseudoInv) { + bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); + bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()); + bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()); + bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); - } - - else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() && - sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { - rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; - } - - else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) && - sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { - rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; - } - - else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && - !(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) { - rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; - } - - else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && - (sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) { - rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; - } - - else { + if (rw1valid && rw2valid && rw3valid && rw4valid) { + std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double)); + return returnvalue::OK; + } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { + std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double)); + return returnvalue::OK; + } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { + std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double)); + return returnvalue::OK; + } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { + std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double)); + return returnvalue::OK; + } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { + std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double)); + return returnvalue::OK; + } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. // Does not make sense, but is implemented that way in MATLAB ?! // Thought: It does not really play a role, because in case there are more then one // reaction wheel invalid the pointing control is destined to fail. - rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + return returnvalue::FAILED; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 0401d07c..32c013fc 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -67,7 +67,7 @@ class Guidance { // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" - void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); private: AcsParameters acsParameters;