eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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], commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime); acsParameters.rwHandlingParameters.rampTime);
return;
} else { } else {
ptgCtrlLostCounter = 0; 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; uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
@ -439,9 +459,8 @@ void AcsController::performPointingCtrl() {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate); targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value, guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorSatRotRate, errorAngle);
errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@ -452,9 +471,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break; break;
@ -462,8 +481,7 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate); targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value, guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat, acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
@ -477,18 +495,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value, guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorSatRotRate, errorAngle);
errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@ -499,9 +516,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break; break;
@ -509,8 +526,7 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgNadirThreeAxes( guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(attitudeEstimationData.quatMekf.value, guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
@ -524,17 +540,16 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
sizeof(targetQuat)); sizeof(targetQuat));
guidance.comparePtg(attitudeEstimationData.quatMekf.value, guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
@ -548,9 +563,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break; break;
default: default: