desaturation cares about disabled wheels now

mission/controller/AcsController.cpp

@@ -433,10 +433,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       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;
 
     case acs::PTG_TARGET:
@@ -456,10 +456,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       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;
 
     case acs::PTG_TARGET_GS:
@@ -476,10 +476,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       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;
 
     case acs::PTG_NADIR:
@@ -498,10 +498,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       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;
 
     case acs::PTG_INERTIAL:
@@ -520,10 +520,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       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;
     default:
       sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;

mission/controller/acs/control/PtgCtrl.cpp

@@ -146,7 +146,8 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
                                       4);
 }
 
-void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgDesaturation(const bool allRwAvailable,
+                              AcsParameters::PointingLawParameters *pointingLawParameters,
                               const double *magFieldB, const bool magFieldBValid,
                               const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
                               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
   // relocate RW speed zero to nullspace RW speed
   double refSpeedRws[4] = {0, 0, 0, 0};
+  if (allRwAvailable) {
     VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
                                         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
   VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
   // convert to rad/s

mission/controller/acs/control/PtgCtrl.h

@@ -38,7 +38,8 @@ class PtgCtrl {
                     const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                     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 int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                        const int32_t speedRw3, double *mgtDpDes);