|
|
|
@ -271,53 +271,106 @@ void AcsController::performControlOperation() {
|
|
|
|
|
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}};
|
|
|
|
|
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
|
|
|
|
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
|
|
|
|
|
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
|
|
|
|
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
|
|
|
|
|
|
|
|
|
|
switch (submode) {
|
|
|
|
|
case SUBMODE_IDLE:
|
|
|
|
|
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
|
|
|
|
targetQuat, refSatRate);
|
|
|
|
|
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
|
|
|
|
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
|
|
|
|
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case SUBMODE_PTG_TARGET:
|
|
|
|
|
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
|
|
|
|
|
targetQuat, refSatRate);
|
|
|
|
|
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
|
|
|
|
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
|
|
|
|
|
|
|
|
|
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case SUBMODE_PTG_TARGET_GS:
|
|
|
|
|
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
|
|
|
|
|
now, targetQuat, refSatRate);
|
|
|
|
|
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
|
|
|
|
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
|
|
|
|
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case SUBMODE_PTG_NADIR:
|
|
|
|
|
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
|
|
|
|
refSatRate);
|
|
|
|
|
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
|
|
|
|
|
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
|
|
|
|
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
ptgCtrl.ptgDesaturation(&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case SUBMODE_PTG_INERTIAL:
|
|
|
|
|
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
|
|
|
|
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double));
|
|
|
|
|
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
|
|
|
|
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
ptgCtrl.ptgDesaturation(&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
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, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
|
|
|
|
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
|
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
|
|
|
|
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
|
|
|
|
|
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
|
|
|
double rwTrqNs[4] = {0, 0, 0, 0};
|
|
|
|
|
ptgCtrl.ptgNullspace(
|
|
|
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
|
|
|
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
|
|
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
|
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
|
|
|
|
|
|
if ( enableAntiStiction ) {
|
|
|
|
|
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
|
|
|
|
@ -333,12 +386,6 @@ void AcsController::performControlOperation() {
|
|
|
|
|
&(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),
|
|
|
|
|
&(sensorValues.rw2Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw3Set.currSpeed.value),
|
|
|
|
|
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
|
|
|
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
|
|
|
|
|
|
|
|
|
|
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
|
|
|
|
|