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

PTG_TARGET should work now
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-17 15:57:07 +01:00
parent 0d32bc0c0a
commit 0dae3b04be
4 changed files with 43 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
mission/controller/AcsController.cpp
View File

@ -228,8 +228,6 @@ void AcsController::performPointingCtrl() {
&mekfData, &validMekf);
uint8_t enableAntiStiction = true;
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}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
@ -245,8 +243,10 @@ void AcsController::performPointingCtrl() {
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0};
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0;
// Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0},
errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0,
satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@ -279,10 +279,17 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle);
guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim,
errorAngle);
if (mekfData.satRotRateMekf.isValid()) {
std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
} else {
std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
}
guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef,
errorQuat, satRotRate, targetSatRotRate,
acsParameters.targetModeControllerParameters.refRotRate, satRotRateError);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),