Merge branch 'main' into mekf-fix

This commit is contained in:
2024-02-29 13:07:41 +01:00
15 changed files with 171 additions and 80 deletions

@@ -400,7 +400,7 @@ void AcsController::performPointingCtrl() {
bool allRwAvailable = true;
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, &rwAvail);
if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@@ -437,10 +437,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_TARGET:
@@ -460,10 +460,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_TARGET_GS:
@@ -480,10 +480,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_NADIR:
@@ -502,10 +502,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_INERTIAL:
@@ -524,10 +524,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
default:
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;