This commit is contained in:
2022-12-13 11:51:03 +01:00
parent d33ae9ede7
commit b49d37e15a
12 changed files with 577 additions and 575 deletions

@ -51,7 +51,7 @@ void AcsController::performControlOperation() {
case SUBMODE_DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_SUN:
case SUBMODE_PTG_SUN:
case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL:
@ -248,9 +248,10 @@ void AcsController::performPointingCtrl() {
&mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
switch (submode) {
switch (submode) {
case SUBMODE_PTG_TARGET:
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat,
refSatRate);
break;
case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
@ -261,7 +262,8 @@ void AcsController::performPointingCtrl() {
case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
break;
} double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
}
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -277,16 +279,18 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),