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

only one guidance function to calculate error quaternion

Browse Source
This commit is contained in:
Marius Eggert
2023-02-20 11:52:53 +01:00
parent 0dae3b04be
commit 700d7ced64
3 changed files with 53 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
mission/controller/AcsController.cpp
View File

@ -246,7 +246,7 @@ void AcsController::performPointingCtrl() {
// 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};
satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@ -279,17 +279,11 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
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,
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),