eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 118 Wiki Activity

select the according quaternion and rotational rate
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-11-24 11:45:10 +01:00
parent 7ef55dcab1
commit f0247a9ab3
1 changed files with 42 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
mission/controller/AcsController.cpp
View File

@ -409,10 +409,30 @@ void AcsController::performPointingCtrl() {
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
ptgCtrlLostCounter = 0;
}
double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
switch (ptgCtrlStrat) {
case acs::ControlModeStrategy::PTGCTRL_MEKF:
std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_STR:
quatBI[0] = sensorValues.strSet.caliQx.value;
quatBI[1] = sensorValues.strSet.caliQy.value;
quatBI[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_QUEST:
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
}
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
@ -439,9 +459,8 @@ void AcsController::performPointingCtrl() {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@ -452,9 +471,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break;
@ -462,8 +481,7 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
@ -477,18 +495,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@ -499,9 +516,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break;
@ -509,8 +526,7 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
@ -524,17 +540,16 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
sizeof(targetQuat));
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
@ -548,9 +563,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
default: