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

added Antistiction, added Nadir Pointing, added performSafe()
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-11-08 13:48:50 +01:00
parent 75ab11fc35
commit 20936faec6
9 changed files with 213 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -16,7 +16,7 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
loadAcsParameters(acsParameters_);
}
@ -164,3 +164,36 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw,
double* torqueCommand) {
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i])
> rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i]
* rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed)
&& (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
}
torqueCommand[i] = torqueMemory[i]
* rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}
}