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

changed antistiction

Browse Source
This commit is contained in:
Marius Eggert
2023-03-10 17:21:52 +01:00
parent 555e0ce49d
commit bce8df376a
4 changed files with 29 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -154,38 +154,33 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
bool rwAvailable[4] = {
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
int32_t currRwSpeed[4] = {
sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) >
acsParameters->rwHandlingParameters.stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque;
}
} else {
if ((omegaRw[i] < acsParameters->rwHandlingParameters.stictionSpeed) &&
(omegaRw[i] > -acsParameters->rwHandlingParameters.stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
if (rwCmdSpeeds[i] != 0) {
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
if (currRwSpeed[i] == 0) {
if (rwCmdSpeeds[i] > 0) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (rwCmdSpeeds[i] < 0) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
} else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}