some things also broken some things fixed again

This commit is contained in:
2023-02-17 14:46:41 +01:00
parent f12fa77644
commit 6352b65f46
3 changed files with 123 additions and 131 deletions

View File

@@ -227,10 +227,7 @@ void AcsController::performPointingCtrl() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 0};
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}};
@@ -248,6 +245,7 @@ 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, 0}, targetSatRotRate[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@@ -273,11 +271,14 @@ void AcsController::performPointingCtrl() {
break;
case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
if (!gpsDataProcessed.gpsPosition.isValid() || !gpsDataProcessed.gpsVelocity.isValid()) {
// ToDo: triggerEvent
return;
}
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,