mission/controller/AcsController.cpp

@@ -267,43 +267,134 @@ void AcsController::performPointingCtrl() {
                      &mekfData, &validMekf);
 
   double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
+  double quatRef[4] = {0, 0, 0, 0};
+  uint8_t enableAntiStiction = true;
+
+  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
+         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
+  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
+  double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
+  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
+
   switch (submode) {
     case SUBMODE_IDLE:
       guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
                           targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      ptgCtrl.ptgNullspace(
+          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+
       break;
+
     case SUBMODE_PTG_TARGET:
       guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
                                       refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      ptgCtrl.ptgNullspace(
+          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
       break;
+
     case SUBMODE_PTG_TARGET_GS:
       guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
                                targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      ptgCtrl.ptgNullspace(
+          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
       break;
+
     case SUBMODE_PTG_NADIR:
       guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
                                      refSatRate);
-      break;
+      std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
-    case SUBMODE_PTG_INERTIAL:
+      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
-      guidance.inertialQuatPtg(targetQuat, refSatRate);
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-      break;
+                          deltaRate);
-  }
+      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
-  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
+                     *rwPseudoInv, torquePtgRws);
-         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
-  guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
-  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
-  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
-  ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
-  double rwTrqNs[4] = {0, 0, 0, 0};
       ptgCtrl.ptgNullspace(
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-  double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
+          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      break;
 
-  if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
+    case SUBMODE_PTG_INERTIAL:
+      guidance.inertialQuatPtg(targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      ptgCtrl.ptgNullspace(
+          &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
+      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      break;
+  }
+
+  if (enableAntiStiction) {
     bool rwAvailable[4] = {true, true, true, true};  // WHICH INPUT SENSOR SET?
     int32_t rwSpeed[4] = {
         (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
@@ -316,12 +407,6 @@ void AcsController::performPointingCtrl() {
                             &(sensorValues.rw2Set.currSpeed.value),
                             &(sensorValues.rw3Set.currSpeed.value),
                             &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
-  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
-  ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
-                          mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
-                          &(sensorValues.rw2Set.currSpeed.value),
-                          &(sensorValues.rw3Set.currSpeed.value),
-                          &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
   actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
 
   int16_t cmdDipolUnitsInt[3] = {0, 0, 0};

mission/controller/acs/AcsParameters.cpp

@@ -374,21 +374,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0xE:
           parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
-        case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.omegaEarth);
-          break;
-        case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.nadirRefDirection);
-          break;
-        case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial);
-          break;
-        case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial);
-          break;
-        case 0x13:
-          parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax);
-          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -817,16 +817,7 @@ class AcsParameters : public HasParametersIF {
 
   } safeModeControllerParameters;
 
-  // ToDo: mutiple structs for different pointing mode controllers?
+  struct PointingLawParameters {
-  struct PointingModeControllerParameters {
-    double refDirection[3] = {-1, 0, 0};  // Antenna
-    double refRotRate[3] = {0, 0, 0};
-    double quatRef[4] = {0, 0, 0, 1};
-    uint8_t avoidBlindStr = true;
-    double blindAvoidStart = 1.5;
-    double blindAvoidStop = 2.5;
-    double blindRotRate = 1 * M_PI / 180;
-
     double zeta = 0.3;
     double om = 0.3;
     double omMax = 1 * M_PI / 180;
@@ -838,14 +829,38 @@ class AcsParameters : public HasParametersIF {
     uint8_t desatOn = true;
     uint8_t enableAntiStiction = true;
 
-    double omegaEarth = 0.000072921158553;
+  } pointingLawParameters;
 
-    double nadirRefDirection[3] = {-1, 0, 0};  // Camera
+  struct TargetModeControllerParameters : PointingLawParameters {
-    double tgtQuatInertial[4] = {0, 0, 0, 1};
+    double refDirection[3] = {-1, 0, 0};  // Antenna
-    double tgtRotRateInertial[3] = {0, 0, 0};
+    double refRotRate[3] = {0, 0, 0};     // Not used atm, do we want an option to
-    int8_t nadirTimeElapsedMax = 10;
+    // give this as an input- currently en calculation is done
-  } pointingModeControllerParameters, inertialModeControllerParameters,
+    double quatRef[4] = {0, 0, 0, 1};
-      nadirModeControllerParameters, targetModeControllerParameters;
+    int8_t timeElapsedMax = 10;  // rot rate calculations
+
+    // Default is Stuttgart GS
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double altitudeTgt = 500;                     // [m]
+
+    // For one-axis control:
+    uint8_t avoidBlindStr = true;
+    double blindAvoidStart = 1.5;
+    double blindAvoidStop = 2.5;
+    double blindRotRate = 1 * M_PI / 180;
+  } targetModeControllerParameters;
+
+  struct NadirModeControllerParameters : PointingLawParameters {
+    double refDirection[3] = {-1, 0, 0};  // Antenna
+    double quatRef[4] = {0, 0, 0, 1};
+    int8_t timeElapsedMax = 10;  // rot rate calculations
+  } nadirModeControllerParameters;
+
+  struct InertialModeControllerParameters : PointingLawParameters {
+    double tgtQuat[4] = {0, 0, 0, 1};
+    double refRotRate[3] = {0, 0, 0};
+    double quatRef[4] = {0, 0, 0, 1};
+  } inertialModeControllerParameters;
 
   struct StrParameters {
     double exclusionAngle = 20 * M_PI / 180;

mission/controller/acs/Guidance.cpp

@@ -48,8 +48,9 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   double targetCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
 
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
@@ -178,17 +179,17 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   }
 }
 
-void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) {
+void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                               double *refSatRate) {
   //-------------------------------------------------------------------------------------
   //	Calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
-  double timeElapsed =
+  double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
-      now.tv_sec + now.tv_usec * pow(10, -6) -
+                       (timeSavedQuaternion.tv_sec +
-      (timeSavedQuaternionNadir.tv_sec +
+                        timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
-       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
+  if (timeElapsed < timeElapsedMax) {
-  if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
     double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
+    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
     VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
 
     double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
@@ -203,21 +204,21 @@ void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double
     VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
 
     VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
+    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
-    omegaRefSavedNadir[0] = omegaRefNew[0];
+    omegaRefSaved[0] = omegaRefNew[0];
-    omegaRefSavedNadir[1] = omegaRefNew[1];
+    omegaRefSaved[1] = omegaRefNew[1];
-    omegaRefSavedNadir[2] = omegaRefNew[2];
+    omegaRefSaved[2] = omegaRefNew[2];
   } else {
     refSatRate[0] = 0;
     refSatRate[1] = 0;
     refSatRate[2] = 0;
   }
 
-  timeSavedQuaternionNadir = now;
+  timeSavedQuaternion = now;
-  savedQuaternionNadir[0] = quatInertialTarget[0];
+  savedQuaternion[0] = quatInertialTarget[0];
-  savedQuaternionNadir[1] = quatInertialTarget[1];
+  savedQuaternion[1] = quatInertialTarget[1];
-  savedQuaternionNadir[2] = quatInertialTarget[2];
+  savedQuaternion[2] = quatInertialTarget[2];
-  savedQuaternionNadir[3] = quatInertialTarget[3];
+  savedQuaternion[3] = quatInertialTarget[3];
 }
 
 void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
@@ -232,8 +233,9 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   double targetCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
   std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
@@ -289,7 +291,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -309,8 +312,9 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
   double groundStationCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart);
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
   double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
@@ -366,7 +370,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -482,9 +487,9 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::
 
   //	rotation quaternion from two vectors
   double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
+  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
+  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[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);
@@ -563,7 +568,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -572,22 +578,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
 }
 
 void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
-  for (int i = 0; i < 4; i++) {
+  std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
-    targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
+              4 * sizeof(double));
-  }
+  std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
-  for (int i = 0; i < 3; i++) {
+              3 * sizeof(double));
-    refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
-  }
 }
 
-void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
-                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
+                          double refSatRate[3], double quatErrorComplete[4], double quatError[3],
-  double quatRef[4] = {0, 0, 0, 0};
+                          double deltaRate[3]) {
-  quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
-  quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
-  quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
-  quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
-
   double satRate[3] = {0, 0, 0};
   std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
   VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);

mission/controller/acs/Guidance.h

@@ -58,10 +58,12 @@ class Guidance {
 
   // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
   // desired
-  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
-                  double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
+                  double refSatRate[3], double quatErrorComplete[4], double quatError[3],
+                  double deltaRate[3]);
 
-  void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate);
+  void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                       double *refSatRate);
 
   //	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
   // reation wheel 	maybe can be done in "commanding.h"
@@ -70,9 +72,9 @@ class Guidance {
  private:
   AcsParameters acsParameters;
   bool strBlindAvoidFlag = false;
-  timeval timeSavedQuaternionNadir;
+  timeval timeSavedQuaternion;
-  double savedQuaternionNadir[4] = {0, 0, 0, 0};
+  double savedQuaternion[4] = {0, 0, 0, 0};
-  double omegaRefSavedNadir[3] = {0, 0, 0};
+  double omegaRefSaved[3] = {0, 0, 0};
 
   static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
   static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";

mission/controller/acs/control/Detumble.cpp

@@ -49,13 +49,11 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
   }
 
   return returnvalue::OK;
-
 }
 
 ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
                                     const double *magField, const bool *magFieldValid,
                                     double *magMom) {
-
   if (!satRateValid || !magFieldValid) {
     return DETUMBLE_NO_SENSORDATA;
   }
@@ -63,5 +61,4 @@ ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateVa
   double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
   VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
   return returnvalue::OK;
-
 }

mission/controller/acs/control/PtgCtrl.cpp

@@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
 PtgCtrl::~PtgCtrl() {}
 
 void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
-  pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
   rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
   rwMatrices = &(acsParameters_->rwMatrices);
 }
 
-void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,
+void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
-                     const double *rwPseudoInv, double *torqueRws) {
+                     const double *qError, const double *deltaRate, const double *rwPseudoInv,
+                     double *torqueRws) {
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
   //------------------------------------------------------------------------------------------------
-  double om = pointingModeControllerParameters->om;
+  double om = pointingLawParameters->om;
-  double zeta = pointingModeControllerParameters->zeta;
+  double zeta = pointingLawParameters->zeta;
-  double qErrorMin = pointingModeControllerParameters->qiMin;
+  double qErrorMin = pointingLawParameters->qiMin;
-  double omMax = pointingModeControllerParameters->omMax;
+  double omMax = pointingLawParameters->omMax;
 
   double cInt = 2 * om * zeta;
   double kInt = 2 * pow(om, 2);
@@ -107,10 +107,11 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
+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) || !(pointingModeControllerParameters->desatOn)) {
+  if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
     mgtDpDes[0] = 0;
     mgtDpDes[1] = 0;
     mgtDpDes[2] = 0;
@@ -128,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
   VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
   //	calculating momentum error
   double deltaMomentum[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(
+  VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
-      momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
+                                     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 = (pointingModeControllerParameters->deSatGainFactor) / normMag;
+  factor = (pointingLawParameters->deSatGainFactor) / normMag;
   VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
 }
 
-void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
+void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                           const int32_t *speedRw0, const int32_t *speedRw1,
                            const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
   double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
   double wheelMomentum[4] = {0, 0, 0, 0};
@@ -150,7 +152,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
   VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
   VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
                                       wheelMomentum, 4);
-  double gainNs = pointingModeControllerParameters->gainNullspace;
+  double gainNs = pointingLawParameters->gainNullspace;
   double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
                                                 *nullSpaceMatrix, 4);

mission/controller/acs/control/PtgCtrl.h

@@ -41,14 +41,16 @@ class PtgCtrl {
   /* @brief: Calculates the needed torque for the pointing control mechanism
    * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
    */
-  void ptgLaw(const double mode, const double *qError, const double *deltaRate,
+  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
-              const double *rwPseudoInv, double *torqueRws);
+              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
 
-  void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
+  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(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
+  void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                    const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
                     const int32_t *speedRw3, double *rwTrqNs);
 
   /* @brief: Commands the stiction torque in case wheel speed is to low
@@ -59,7 +61,6 @@ class PtgCtrl {
   void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
 
  private:
-  AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
   AcsParameters::RwHandlingParameters *rwHandlingParameters;
   AcsParameters::InertiaEIVE *inertiaEIVE;
   AcsParameters::RwMatrices *rwMatrices;