mission/controller/acs/control/PtgCtrl.cpp

@@ -95,43 +95,6 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
-                              double *magFieldEst, bool magFieldEstValid, double *satRate,
-                              int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
-                              int32_t *speedRw3, double *mgtDpDes) {
-  if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
-    mgtDpDes[0] = 0;
-    mgtDpDes[1] = 0;
-    mgtDpDes[2] = 0;
-    return;
-  }
-
-  // concentrate RW speeds as vector and convert to double
-  double speedRws[4] = {static_cast<double>(*speedRw0), static_cast<double>(*speedRw1),
-                        static_cast<double>(*speedRw2), static_cast<double>(*speedRw3)};
-
-  // calculating momentum of satellite and momentum of reaction wheels
-  double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
-                                      momentumRwU, 4);
-  MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
-                                     momentumRw, 3, 4, 1);
-  double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
-                                     momentumSat, 3, 3, 1);
-  VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
-  // calculating momentum error
-  double deltaMomentum[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
-                                     deltaMomentum, 3);
-  // resulting magnetic dipole command
-  double crossMomentumMagField[3] = {0, 0, 0};
-  VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
-  double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
-  factor = (pointingLawParameters->deSatGainFactor) / normMag;
-  VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
-}
-
 void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
                            const int32_t *speedRw0, const int32_t *speedRw1,
                            const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
@@ -161,6 +124,61 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
                                       4);
 }
 
+void PtgCtrl::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) {
+  if (not magFieldBValid or not pointingLawParameters->desatOn) {
+    return;
+  }
+
+  // concentrate RW speeds as vector and convert to double
+  double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
+                        static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
+
+  // convert magFieldB from uT to T
+  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);
+  VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
+  // convert to rad/s
+  VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
+  // calculate angular momentum of each RW
+  double angMomentumRwU[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
+                                      angMomentumRwU, 4);
+  // convert RW angular momentum to body RF
+  double angMomentumRw[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
+                                     angMomentumRw, 3, 4, 1);
+
+  // 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);
+
+  // resulting magnetic dipole command
+  double crossAngMomentumMagField[3] = {0, 0, 0};
+  VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
+  double factor =
+      pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
+  VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
+}
+
 void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
   bool rwAvailable[4] = {
       (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),

mission/controller/acs/control/PtgCtrl.h

@@ -26,15 +26,15 @@ class PtgCtrl {
   void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
               const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
 
-  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
-                       double *magFieldEst, bool magFieldEstValid, double *satRate,
-                       int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
-                       double *mgtDpDes);
-
   void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
                     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);
+
   /* @brief: Commands the stiction torque in case wheel speed is to low
    * 		 torqueCommand  modified torque after antistiction
    */