eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

desaturation cares about disabled wheels now
Some checks are pending
EIVE/eive-obsw/pipeline/head Build queued...
Details
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-02-19 14:54:17 +01:00
parent 77527c631c
commit 026776c1ec
3 changed files with 32 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
mission/controller/AcsController.cpp
View File

@ -433,10 +433,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &acsParameters.idleModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
@ -456,10 +456,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &acsParameters.targetModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
@ -476,10 +476,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_NADIR: case acs::PTG_NADIR:
@ -498,10 +498,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &acsParameters.nadirModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
@ -520,10 +520,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &acsParameters.inertialModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
default: default:
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;

14
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -146,7 +146,8 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
4); 4);
} }
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgDesaturation(const bool allRwAvailable,
AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *magFieldB, const bool magFieldBValid,
const double *satRate, const int32_t speedRw0, const int32_t speedRw1, const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
@ -170,9 +171,14 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
// calculate angular momentum of the reaction wheels with respect to the nullspace RW speed // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
// relocate RW speed zero to nullspace RW speed // relocate RW speed zero to nullspace RW speed
double refSpeedRws[4] = {0, 0, 0, 0}; double refSpeedRws[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, if (allRwAvailable) {
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4); pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
} else if (VectorOperations<double>::maxAbsValue(speedRws, 4) <
pointingLawParameters->nullspaceSpeed) {
return;
}
// convert speed from 10 RPM to 1 RPM // convert speed from 10 RPM to 1 RPM
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4); VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
// convert to rad/s // convert to rad/s

3
mission/controller/acs/control/PtgCtrl.h
View File

@ -38,7 +38,8 @@ class PtgCtrl {
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgDesaturation(const bool allRwAvabilable,
AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *satRate, const double *magFieldB, const bool magFieldBValid, const double *satRate,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *mgtDpDes); const int32_t speedRw3, double *mgtDpDes);