Merge pull request 'Fix Desaturation for Faulty Wheels' (#865) from fix-desaturation into main

Reviewed-on: #865
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -29,6 +29,7 @@ will consitute of a breaking change warranting a new major release:
 - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
   and is triggered by the `AcsController` now.
 - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
+- Fixed calculation of desaturation torque for faulty RWs.
 
 ## Changed
 
@@ -42,6 +43,7 @@ will consitute of a breaking change warranting a new major release:
   also limits the rotation for the reference target quaternion to prevent spikes in required
   rotation rates.
 - Updated QUEST and Sun Vector Params to new values.
+- Removed the satellites's angular momentum from desaturation calculation.
 
 ## Added
 

mission/controller/AcsController.cpp

@@ -396,7 +396,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) {
@@ -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,
-          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:
@@ -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,
-          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:
@@ -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,
-          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:
@@ -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,
-          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:
@@ -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,
-          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;

mission/controller/AcsController.h

@@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipoleMtqs[3] = {0, 0, 0};
 
+  acsctrl::RwAvail rwAvail;
+
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif

mission/controller/acs/Guidance.cpp

@@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
-                                                double *rwPseudoInv) {
-  bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
-  bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
-  bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
-  bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
+                                                double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
+  rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
+  rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
+  rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
+  rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
 
-  if (rw1valid and rw2valid and rw3valid and rw4valid) {
+  if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
+  } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
+  } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
+  } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
+  } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
+             not rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;

mission/controller/acs/Guidance.h

@@ -44,7 +44,8 @@ class Guidance {
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                   double &errorAngle);
 
-  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
+  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
+                                        acsctrl::RwAvail *rwAvail);
 
  private:
   const AcsParameters *acsParameters;

mission/controller/acs/control/PtgCtrl.cpp

@@ -1,11 +1,5 @@
 #include "PtgCtrl.h"
 
-#include <fsfw/globalfunctions/constants.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <fsfw/globalfunctions/sign.h>
-
 PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 PtgCtrl::~PtgCtrl() {}
@@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
 
   // Inverse of gainMatrix
   double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
-  gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
-  gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
+  gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
+  gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
+  gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
 
   double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MatrixOperations<double>::multiply(
@@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
                                       4);
 }
 
-void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail,
+                              AcsParameters::PointingLawParameters *pointingLawParameters,
                               const double *magFieldB, const bool magFieldBValid,
-                              const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
+                              const int32_t speedRw0, const int32_t speedRw1,
                               const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
   if (not magFieldBValid or not pointingLawParameters->desatOn) {
     return;
@@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  // calculate angular momentum of the satellite
-  double angMomentumSat[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
-                                     angMomentumSat, 3, 3, 1);
-
   // 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};
   VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
                                       pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
+  if (not allRwAvailable) {
+    if (not rwAvail->rw1avail) {
+      refSpeedRws[0] = 0.0;
+    } else if (not rwAvail->rw2avail) {
+      refSpeedRws[1] = 0.0;
+    } else if (not rwAvail->rw3avail) {
+      refSpeedRws[2] = 0.0;
+    } else if (not rwAvail->rw4avail) {
+      refSpeedRws[3] = 0.0;
+    }
+  }
   VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
+
   // convert speed from 10 RPM to 1 RPM
   VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
   // convert to rad/s
@@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
 
   // calculate total angular momentum
   double angMomentumTotal[3] = {0, 0, 0};
-  VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
-
-  // calculating momentum error
-  double deltaAngMomentum[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
-                                     deltaAngMomentum, 3);
+  VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
+                                     angMomentumTotal, 3);
 
   // resulting magnetic dipole command
   double crossAngMomentumMagField[3] = {0, 0, 0};
-  VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
+  VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
   double factor =
       pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
   VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);

mission/controller/acs/control/PtgCtrl.h

@@ -1,11 +1,16 @@
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_
 
-#include <math.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/globalfunctions/sign.h>
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/SensorValues.h>
-#include <stdio.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+
+#include <cmath>
 
 class PtgCtrl {
   /*
@@ -38,10 +43,11 @@ 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,
-                       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);
+  void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
+                       AcsParameters::PointingLawParameters *pointingLawParameters,
+                       const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
+                       const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
+                       double *mgtDpDes);
 
   /* @brief: Commands the stiction torque in case wheel speed is to low
    * 		 torqueCommand  modified torque after anti-stiction

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
 //! [EXPORT] : [COMMENT] Multiple RWs have failed.
 static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
 
+struct RwAvail {
+  bool rw1avail = false;
+  bool rw2avail = false;
+  bool rw3avail = false;
+  bool rw4avail = false;
+};
+
 enum SetIds : uint32_t {
   MGM_SENSOR_DATA,
   MGM_PROCESSED_DATA,