some things also broken some things fixed again
This commit is contained in:
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user