@ -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),
|
||||
|
Reference in New Issue
Block a user