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

detumble fixes and improvements
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2024-02-29 10:25:02 +01:00
parent a2a360c1d7
commit 557051ba37
7 changed files with 66 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
if (rw1valid and rw2valid and rw3valid and rw4valid) {
if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
} else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
} else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
} else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
} else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
not rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;