Merge pull request 'Fixed units in ACS Control Laws Safe and Detumble' (#421) from acs-units-fix into develop

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

CHANGELOG.md

@@ -26,6 +26,7 @@ will consitute of a breaking change warranting a new major release:
 
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
 - Set RM3100 dataset to valid.
+- Fixed units in calculation of ACS control laws safe and detumble.
 
 ## Added
 

mission/controller/AcsController.cpp

@@ -157,20 +157,23 @@ void AcsController::performSafe() {
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  bool magMomMtqValid = false;
   if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
-    safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
+    result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
-                      mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
+                               mgmDataProcessed.magIgrfModel.value,
-                      susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
+                               mgmDataProcessed.magIgrfModel.isValid(),
-                      mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
+                               susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
-                      sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+                               mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
+                               sunTargetDir, satRateSafe, &errAng, magMomMtq);
   } else {
-    safeCtrl.safeNoMekf(
+    result = safeCtrl.safeNoMekf(
         now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
         susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
         mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
         mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
-        sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+        sunTargetDir, satRateSafe, &errAng, magMomMtq);
+  }
+  if (result == returnvalue::FAILED) {
+    // ToDo: this should never ever happen or we are dead. prob add an event at least
   }
 
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);

mission/controller/acs/control/Detumble.cpp

@@ -1,11 +1,3 @@
-
-/*
- * Detumble.cpp
- *
- *  Created on: 17 Aug 2022
- *      Author: Robin Marquardt
- */
-
 #include "Detumble.h"
 
 #include <fsfw/globalfunctions/constants.h>
@@ -31,6 +23,12 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
   if (!magRateValid || !magFieldValid) {
     return DETUMBLE_NO_SENSORDATA;
   }
+
+  // change unit from uT to T
+  double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
+  VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
+
   double gain = detumbleParameter->gainD;
   double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
   VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);

mission/controller/acs/control/SafeCtrl.cpp

@@ -32,15 +32,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                  double *magFieldModel, bool magFieldModelValid,
                                  double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
                                  bool rateMekfValid, double *sunDirRef, double *satRatRef,
-                                 double *outputAngle, double *outputMagMomB, bool *outputValid) {
+                                 double *outputAngle, double *outputMagMomB) {
   if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
-    *outputValid = false;
     return SAFECTRL_MEKF_INPUT_INVALID;
   }
 
-  double kRate = 0, kAlign = 0;
+  double kRate = safeModeControllerParameters->k_rate_mekf;
-  kRate = safeModeControllerParameters->k_rate_mekf;
+  double kAlign = safeModeControllerParameters->k_align_mekf;
-  kAlign = safeModeControllerParameters->k_align_mekf;
 
   //	Calc sunDirB ,magFieldB with mekf output and model
   double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -49,22 +47,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
 
-  double crossSun[3] = {0, 0, 0};
+  // change unit from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
 
+  double crossSun[3] = {0, 0, 0};
   VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
   double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
 
   //	calc angle alpha between sunDirRef and sunDIr
-  double alpha = 0, dotSun = 0;
+  double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
-  dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
+  double alpha = acos(dotSun);
-  alpha = acos(dotSun);
 
   //	Law Torque calculations
   double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
          torqueAll[3] = {0, 0, 0};
 
-  double scalarFac = 0;
+  double scalarFac = kAlign * alpha / normCrossSun;
-  scalarFac = kAlign * alpha / normCrossSun;
   VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
 
   double rateSafeMode[3] = {0, 0, 0};
@@ -82,23 +80,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
 
   *outputAngle = alpha;
-  *outputValid = true;
-
   return returnvalue::OK;
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
                           double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB,
+                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
-                          bool *outputValid) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
-    *outputValid = false;
+    return returnvalue::FAILED;
-    return;
   }
 
+  // change unit from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
+
   // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(magFieldB, magDirB, 3);
@@ -108,13 +105,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
 
   // Rate parallel to sun direction and magnetic field direction
-  double rateParaSun = 0, rateParaMag = 0;
+  double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
-  double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
+  double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
-  dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
+  double rateFactor = 1 - pow(cosAngleSunMag, 2);
-  dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
+  double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
-  rateFactor = 1 - pow(cosAngleSunMag, 2);
+  double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
-  rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
-  rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
 
   // Full rate or estimate
   double estSatRate[3] = {0, 0, 0};
@@ -130,7 +125,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
    * is sufficiently large */
   double angleSunMag = acos(cosAngleSunMag);
   if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
-    return;
+    return returnvalue::FAILED;
   }
 
   // Rate for Torque Calculation
@@ -138,9 +133,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
 
   // Torque Align calculation
-  double kRateNoMekf = 0, kAlignNoMekf = 0;
+  double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
-  kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
+  double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
-  kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
 
   double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
   double crossSusSunRef[3] = {0, 0, 0};
@@ -171,5 +165,5 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
 
   std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
   *outputAngle = angleAlignErr;
-  *outputValid = true;
+  return returnvalue::OK;
 }

mission/controller/acs/control/SafeCtrl.h

@@ -23,14 +23,12 @@ class SafeCtrl {
                          bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
                          double *satRateMekf, bool rateMekfValid, double *sunDirRef,
                          double *satRatRef,  // From Guidance (!)
-                         double *outputAngle, double *outputMagMomB, bool *outputValid);
+                         double *outputAngle, double *outputMagMomB);
 
-  void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+  ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                  bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
+                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                  bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
+                           double *magRateB, bool magRateBValid, double *sunDirRef,
-                  double *outputMagMomB, bool *outputValid);
+                           double *satRateRef, double *outputAngle, double *outputMagMomB);
-
-  void idleSunPointing();  // with reaction wheels
 
  protected:
  private: