eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

RW pseudo inverses now set from RW state. multiple invalids trigger event and disable ptgCtrl
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-16 15:40:16 +01:00
parent 4b8f5992b5
commit 97f7f7c973
4 changed files with 31 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mission/acsDefs.h
View File

@ -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); static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
//!< The system has recovered from a safe rate rotation violation. //!< The system has recovered from a safe rate rotation violation.
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); 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); extern const char* getModeStr(AcsMode mode);

6
mission/controller/AcsController.cpp
View File

@ -263,7 +263,11 @@ void AcsController::performPointingCtrl() {
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed 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}}; 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 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 torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0}; double mgtDpDes[3] = {0, 0, 0};

117
mission/controller/acs/Guidance.cpp
View File

@ -610,104 +610,33 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
// under 150 arcsec ?? // under 150 arcsec ??
} }
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && double *rwPseudoInv) {
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
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];
} if (rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() && return returnvalue::OK;
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; return returnvalue::OK;
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; return returnvalue::OK;
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; return returnvalue::OK;
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; return returnvalue::OK;
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; } else {
}
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 {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid. // @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?! // 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 // 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. // reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; return returnvalue::FAILED;
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];
} }
} }

2
mission/controller/acs/Guidance.h
View File

@ -67,7 +67,7 @@ class Guidance {
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h" // 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: private:
AcsParameters acsParameters; AcsParameters acsParameters;