mission/controller/AcsController.cpp

@@ -6,9 +6,6 @@
 
 AcsController::AcsController(object_id_t objectId)
     : ExtendedControllerBase(objectId),
-      sensorProcessing(&acsParameters),
-      navigation(&acsParameters),
-      actuatorCmd(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       detumble(&acsParameters),
@@ -142,7 +139,7 @@ void AcsController::performSafe() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
     if (not mekfInvalidFlag) {
@@ -173,7 +170,7 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
+  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
 
   // detumble check and switch
   if (mekfData.satRotRateMekf.isValid() &&
@@ -207,7 +204,7 @@ void AcsController::performDetumble() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
     if (not mekfInvalidFlag) {
@@ -221,7 +218,7 @@ void AcsController::performDetumble() {
   detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                    mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
                    mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
+  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
 
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@@ -254,7 +251,7 @@ void AcsController::performPointingCtrl() {
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
     if (not mekfInvalidFlag) {
@@ -303,7 +300,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
       ptgCtrl.ptgDesaturation(
           &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -327,7 +324,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -348,7 +345,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -372,7 +369,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
       ptgCtrl.ptgDesaturation(
           &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -395,7 +392,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
       ptgCtrl.ptgDesaturation(
           &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -409,11 +406,11 @@ void AcsController::performPointingCtrl() {
     ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
   }
 
-  actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
+  actuatorCmd.cmdSpeedToRws(
-                            sensorValues.rw2Set.currSpeed.value,
+      sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-                            sensorValues.rw3Set.currSpeed.value,
+      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled,
-                            sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
+      cmdSpeedRws, &acsParameters);
-  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
+  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
 
   updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
   updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);

mission/controller/acs/ActuatorCmd.cpp

@@ -56,7 +56,6 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
 
 void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
                               AcsParameters::MagnetorquerParameter *magnetorquerParameter) {
-  sif::debug << magnetorquerParameter->dipolMax << std::endl;
   // Convert to actuator frame
   double dipolMomentActuatorDouble[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment,

mission/controller/acs/Guidance.cpp

@@ -12,7 +12,7 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
 
-Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
+Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 Guidance::~Guidance() {}
 
@@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
   double targetE[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters->targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
 
   // target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
@@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
 
   // rotation quaternion from two vectors
   double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
+  refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
+  refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
+  refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
   double noramlizedTargetDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
   VectorOperations<double>::normalize(refDir, refDir, 3);
@@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
   //-------------------------------------------------------------------------------------
   //	Calculation of reference rotation rate in case of star tracker blinding
   //-------------------------------------------------------------------------------------
-  if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
+  if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
     double sunDirB[3] = {0, 0, 0};
     MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
 
-    double exclAngle = acsParameters.strParameters.exclusionAngle,
+    double exclAngle = acsParameters->strParameters.exclusionAngle,
-           blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
+           blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
-           blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
+           blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
     double sightAngleSun =
-        VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
+        VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
 
     if (!(strBlindAvoidFlag)) {
       double critSightAngle = blindStart * exclAngle;
@@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
       }
     } else {
       if (sightAngleSun < blindEnd * exclAngle) {
-        double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
+        double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
         double blindRefRate[3] = {0, 0, 0};
         if (sunDirB[1] < 0) {
           blindRefRate[0] = normBlindRefRate;
@@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
   // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
   double targetE[3] = {0, 0, 0};
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters->targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
 
@@ -198,12 +198,13 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
                         {xAxis[2], yAxis[2], zAxis[2]}};
   QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
   targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
 
 void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
                                double targetQuat[4], double targetSatRotRate[3]) {
+  sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl;
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for ground station pointing
   //-------------------------------------------------------------------------------------
@@ -211,9 +212,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
   double groundStationE[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.gsTargetModeControllerParameters.latitudeTgt,
+      acsParameters->gsTargetModeControllerParameters.latitudeTgt,
-      acsParameters.gsTargetModeControllerParameters.longitudeTgt,
+      acsParameters->gsTargetModeControllerParameters.longitudeTgt,
-      acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
+      acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
 
@@ -262,7 +263,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
                          {xAxis[2], yAxis[2], zAxis[2]}};
   QuaternionOperations::fromDcm(dcmTgt, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
   targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
 
@@ -332,9 +333,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
 
   // rotation quaternion from two vectors
   double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
+  refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
+  refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
+  refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
   double noramlizedTargetDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
   VectorOperations<double>::normalize(refDir, refDir, 3);
@@ -406,7 +407,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
                          {xAxis[2], yAxis[2], zAxis[2]}};
   QuaternionOperations::fromDcm(dcmTgt, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
   targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
 }
 
@@ -516,19 +517,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
   bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
 
   if (rw1valid && rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
   } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
     return returnvalue::OK;
   } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
     return returnvalue::OK;
   } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
     return returnvalue::OK;
   } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
     return returnvalue::OK;
   } else {
     // 	    @note: This one takes the normal pseudoInverse of all four raction wheels valid.
@@ -542,13 +543,13 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
   if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
       not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {  // ToDo: if file does not exist anymore
-    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
+    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
                 3 * sizeof(double));
   } else {
-    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
+    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
                 3 * sizeof(double));
   }
-  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
+  std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
               3 * sizeof(double));
 }
 

mission/controller/acs/Guidance.h

@@ -55,7 +55,8 @@ class Guidance {
   ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 
  private:
-  AcsParameters acsParameters;
+  const AcsParameters *acsParameters;
+
   bool strBlindAvoidFlag = false;
   timeval timeSavedQuaternion;
   double savedQuaternion[4] = {0, 0, 0, 0};