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
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
4b8f5992b5
commit
97f7f7c973
@ -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);
|
||||
|
||||
|
@ -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};
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user