CHANGELOG.md

@@ -62,6 +62,7 @@ will consitute of a breaking change warranting a new major release:
 - ACU dummy HK sets
 - IMTQ HK sets
 - IMTQ dummy now handles power switch
+- Added some new ACS parameters
 
 ## Fixed
 
@@ -96,6 +97,8 @@ will consitute of a breaking change warranting a new major release:
 - STR datasets were not set to invalid on shutdown.
 - Fixed usage of quaternion valid flag, which does not actually represent the validity of the
   quaternion.
+- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as
+  intended.
 
 # [v2.0.5] 2023-05-11
 

mission/controller/AcsController.cpp

@@ -169,7 +169,7 @@ void AcsController::performSafe() {
   guidance.getTargetParamsSafe(sunTargetDir);
 
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
+  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
       acsParameters.safeModeControllerParameters.useMekf,
@@ -205,11 +205,13 @@ void AcsController::performSafe() {
     case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
+    default:
+      sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
+      break;
   }
 
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
-                          acsParameters.magnetorquerParameter.dipolMax);
 
   // detumble check and switch
   if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
@@ -231,8 +233,8 @@ void AcsController::performSafe() {
   }
 
   updateCtrlValData(errAng, safeCtrlStrat);
-  updateActuatorCmdData(cmdDipolMtqs);
+  updateActuatorCmdData(cmdDipoleMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration);
 }
 
@@ -259,7 +261,7 @@ void AcsController::performDetumble() {
     triggerEvent(acs::MEKF_RECOVERY);
     mekfInvalidFlag = false;
   }
-  uint8_t safeCtrlStrat = detumble.detumbleStrategy(
+  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
       mgmDataProcessed.mgmVecTotDerivative.isValid(),
       acsParameters.detumbleParameter.useFullDetumbleLaw);
@@ -279,11 +281,13 @@ void AcsController::performDetumble() {
     case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
+    default:
+      sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
+      break;
   }
 
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
-                          acsParameters.magnetorquerParameter.dipolMax);
 
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@@ -304,8 +308,8 @@ void AcsController::performDetumble() {
   }
 
   disableCtrlValData();
-  updateActuatorCmdData(cmdDipolMtqs);
+  updateActuatorCmdData(cmdDipoleMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration);
 }
 
@@ -366,24 +370,26 @@ void AcsController::performPointingCtrl() {
   // Variables required for setting actuators
   double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
          mgtDpDes[3] = {0, 0, 0};
+
   switch (mode) {
     case acs::PTG_IDLE:
-      guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
+                                targetSatRotRate);
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                           targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
+      ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
-          &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+                           rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
       break;
 
@@ -397,17 +403,17 @@ void AcsController::performPointingCtrl() {
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
+      ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+                           rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
@@ -418,17 +424,17 @@ void AcsController::performPointingCtrl() {
                           targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
+      ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
-          &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+                           rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
       break;
 
@@ -442,63 +448,61 @@ void AcsController::performPointingCtrl() {
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
+      ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
-          &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+                           rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
       std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
-                  4 * sizeof(double));
+                  sizeof(targetQuat));
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                           targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
                           acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
+      ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
-          &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+                           rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
     default:
-      sif::error << "AcsController: Invalid mode for performPointingCtrl";
+      sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
       break;
   }
 
   actuatorCmd.cmdSpeedToRws(
       sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
+      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-      cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
+      acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
-      acsParameters.rwHandlingParameters.maxRwSpeed,
+      acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
-      acsParameters.rwHandlingParameters.inertiaWheel);
   if (enableAntiStiction) {
     ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
   }
-  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
-                          acsParameters.magnetorquerParameter.dipolMax);
 
   updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
-  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
+  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
                    cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                    acsParameters.rwHandlingParameters.rampTime);

mission/controller/AcsController.h

@@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   bool mekfLost = false;
 
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+  int16_t cmdDipoleMtqs[3] = {0, 0, 0};
 
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;

mission/controller/acs/AcsParameters.cpp

@@ -318,7 +318,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setMatrix(rwMatrices.without4);
           break;
         case 0x6:
-          parameterWrapper->setVector(rwMatrices.nullspace);
+          parameterWrapper->setVector(rwMatrices.nullspaceVector);
           break;
         default:
           return INVALID_IDENTIFIER_ID;
@@ -378,15 +378,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(idleModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
+          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          break;
+        case 0x9:
           parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
           break;
         default:
@@ -411,48 +414,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(targetModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
           break;
         case 0xB:
-          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
+          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
           break;
         case 0xC:
-          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
           break;
         case 0xD:
-          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
           break;
         case 0xE:
-          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
           break;
         case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
           break;
         case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
           break;
         case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
+          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
           break;
         case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
           break;
         case 0x13:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          break;
+        case 0x14:
           parameterWrapper->set(targetModeControllerParameters.blindRotRate);
           break;
         default:
@@ -477,30 +483,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
+          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
+          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
           break;
         case 0xB:
-          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
           break;
         case 0xC:
-          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
           break;
         case 0xD:
+          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xE:
           parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
           break;
         default:
@@ -525,27 +534,30 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(nadirModeControllerParameters.desatOn);
+          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(nadirModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
+          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
+          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
           break;
         case 0xB:
-          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
           break;
         case 0xC:
+          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          break;
+        case 0xD:
           parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
           break;
         default:
@@ -570,21 +582,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(inertialModeControllerParameters.desatOn);
+          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(inertialModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
+          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          break;
+        case 0xB:
           parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
           break;
         case 0xC:
@@ -696,7 +711,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
           break;
         case 0x5:
-          parameterWrapper->set(magnetorquerParameter.dipolMax);
+          parameterWrapper->set(magnetorquerParameter.dipoleMax);
           break;
         case 0x6:
           parameterWrapper->set(magnetorquerParameter.torqueDuration);

mission/controller/acs/AcsParameters.h

@@ -816,7 +816,7 @@ class AcsParameters : public HasParametersIF {
         {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
     double without4[4][3] = {
         {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
-    double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
+    double nullspaceVector[4] = {-1, 1, -1, 1};
   } rwMatrices;
 
   struct SafeModeControllerParameters {
@@ -840,7 +840,9 @@ class AcsParameters : public HasParametersIF {
     double om = 0.3;
     double omMax = 1 * M_PI / 180;
     double qiMin = 0.1;
+
     double gainNullspace = 0.01;
+    double nullspaceSpeed = 32500;  // 0.1 RPM
 
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
@@ -933,7 +935,7 @@ class AcsParameters : public HasParametersIF {
     double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
     double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
     double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
-    double dipolMax = 0.2;  // [Am^2]
+    double dipoleMax = 0.2;  // [Am^2]
 
     uint16_t torqueDuration = 300;  // [ms]
   } magnetorquerParameter;

mission/controller/acs/ActuatorCmd.cpp

@@ -5,11 +5,6 @@
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 
-#include <cmath>
-
-#include "util/CholeskyDecomposition.h"
-#include "util/MathOperations.h"
-
 ActuatorCmd::ActuatorCmd() {}
 
 ActuatorCmd::~ActuatorCmd() {}
@@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
   }
 }
 
-void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
+void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
-                                int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
+                                const int32_t speedRw2, const int32_t speedRw3,
-                                double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
+                                const double sampleTime, const double inertiaWheel,
-  using namespace Math;
+                                const int32_t maxRwSpeed, const double *rwTorque,
-
+                                int32_t *rwCmdSpeed) {
-  // Calculating the commanded speed in RPM for every reaction wheel
+  // group RW speed values (in 0.1 [RPM]) in vector
   int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
+
+  // calculate required RW speed as sum of current RW speed and RW speed delta
+  // delta w_rw = delta t / I_RW * torque_RW [rad/s]
   double deltaSpeed[4] = {0, 0, 0, 0};
-  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
+  const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
-  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
-  double factor = sampleTime / inertiaWheel * radToRpm;
-  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
   VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
+
+  // convert double to int32
+  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
   for (int i = 0; i < 4; i++) {
     deltaSpeedInt[i] = std::round(deltaSpeed[i]);
   }
+
+  // sum of current RW speed and RW speed delta
   VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
-  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
+
+  // crop values which would exceed the maximum possible RPM
   for (uint8_t i = 0; i < 4; i++) {
     if (rwCmdSpeed[i] > maxRwSpeed) {
       rwCmdSpeed[i] = maxRwSpeed;
@@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
   }
 }
 
-void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
+void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
-                              const double *inverseAlignment, double maxDipol) {
+                               const double *dipoleMoment, int16_t *dipoleMomentActuator) {
-  // Convert to actuator frame
+  // convert to actuator frame
-  double dipolMomentActuatorDouble[3] = {0, 0, 0};
+  double dipoleMomentActuatorDouble[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
+  MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3,
-                                     1);
+                                     3, 1);
-  // Scaling along largest element if dipol exceeds maximum
+  // scaling along largest element if dipole exceeds maximum
   uint8_t maxIdx = 0;
-  VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
+  VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
-  double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]);
+  double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]);
-  if (maxAbsValue > maxDipol) {
+  if (maxAbsValue > maxDipole) {
-    double scalingFactor = maxDipol / maxAbsValue;
+    double scalingFactor = maxDipole / maxAbsValue;
-    VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
+    VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
-                                        dipolMomentActuatorDouble, 3);
+                                        dipoleMomentActuatorDouble, 3);
   }
   // scale dipole from 1 Am^2 to 1e^-4 Am^2
-  VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
+  VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
+                                      3);
   for (int i = 0; i < 3; i++) {
-    dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
+    dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
   }
 }

mission/controller/acs/ActuatorCmd.h

@@ -1,9 +1,8 @@
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_
 
-#include "MultiplicativeKalmanFilter.h"
+#include <cmath>
-#include "SensorProcessing.h"
+
-#include "SensorValues.h"
 
 class ActuatorCmd {
  public:
@@ -19,29 +18,30 @@ class ActuatorCmd {
   void scalingTorqueRws(double *rwTrq, double maxTorque);
 
   /*
-   * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
+   * @brief:	cmdSpeedToRws()		Calculates the RPM for the reaction wheel configuration,
-   * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
+   * given the required torque calculated by the controller. Will also scale down the RPM of the
-   * as Input to the RWs
+   * wheels if they exceed the maximum possible RPM
-   * @param:	rwTrqIn 			given torque from pointing controller
+   * @param:	rwTrq 			    given torque from pointing controller
-   * 			rwTrqNS				Nullspace torque
    * 			rwCmdSpeed			output revolutions per minute for every
    * reaction wheel
    */
-  void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
+  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
-                     const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
+                     const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
-                     int32_t maxRwSpeed, double inertiaWheel);
+                     const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
 
   /*
-   * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
+   * @brief:	cmdDipoleMtq()			gives the commanded dipole moment for the
+   * magnetorquer
    *
-   * @param:	dipolMoment 		given dipol moment in spacecraft frame
+   * @param:	dipoleMoment 			given dipole moment in spacecraft frame
-   * 			dipolMomentActuator	resulting dipol moment in actuator reference frame
+   * 			dipoleMomentActuator	resulting dipole moment in actuator reference frame
    */
-  void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
+  void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
-                   const double *inverseAlignment, double maxDipol);
+                    const double *dipoleMoment, int16_t *dipoleMomentActuator);
 
  protected:
  private:
+  static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
 };
 
 #endif /* ACTUATORCMD_H_ */

mission/controller/acs/Guidance.cpp

@@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
   targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                                double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to sun
   //-------------------------------------------------------------------------------------
@@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
   //----------------------------------------------------------------------------
   //	Calculation of reference rotation rate
   //----------------------------------------------------------------------------
-  refSatRate[0] = 0;
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
-  refSatRate[1] = 0;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
-  refSatRate[2] = 0;
 }
 
 void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
@@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
 
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
-                          double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
+                          double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
   // First calculate error quaternion between current and target orientation
   QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
   // Last calculate add rotation from reference quaternion
@@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   // Calculate error angle
   errorAngle = QuaternionOperations::getAngle(errorQuat, true);
 
-  // Only give back error satellite rotational rate if orientation has already been aquired
+  // Calculate error satellite rotational rate
-  if (errorAngle < 2. / 180. * M_PI) {
   // First combine the target and reference satellite rotational rates
   double combinedRefSatRotRate[3] = {0, 0, 0};
   VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
-    // Then substract the combined required satellite rotational rates from the actual rate
+  // Then subtract the combined required satellite rotational rates from the actual rate
-    VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
+  VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
-                                       3);
-  } else {
-    // If orientation has not been aquired yet set satellite rotational rate to zero
-    errorSatRotRate = 0;
-  }
-
-  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  // under 150 arcsec ??
 }
 
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double errorQuat[4],
-                          double errorSatRotRate[3], double errorAngle) {
+                          double errorSatRotRate[3], double &errorAngle) {
   // First calculate error quaternion between current and target orientation
   QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
   // Keep scalar part of quaternion positive
@@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   // Calculate error angle
   errorAngle = QuaternionOperations::getAngle(errorQuat, true);
 
-  // Only give back error satellite rotational rate if orientation has already been aquired
+  // Calculate error satellite rotational rate
-  if (errorAngle < 2. / 180. * M_PI) {
-    // Then substract the combined required satellite rotational rates from the actual rate
   VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
-  } else {
-    // If orientation has not been aquired yet set satellite rotational rate to zero
-    errorSatRotRate = 0;
-  }
-
-  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  // under 150 arcsec ??
 }
 
 void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
@@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
   //-------------------------------------------------------------------------------------
   //	Calculation of target rotation rate
   //-------------------------------------------------------------------------------------
-  double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
+  double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
-                       (timeSavedQuaternion.tv_sec +
+                       (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
-                        timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
+  if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
+    std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
+  }
   if (timeElapsed < timeElapsedMax) {
+    double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
+    QuaternionOperations::inverse(quatInertialTarget, q);
+    QuaternionOperations::inverse(savedQuaternion, qS);
     double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
+    VectorOperations<double>::subtract(q, qS, qDiff, 4);
     VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
 
-    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
+    double tgtQuatVec[3] = {q[0], q[1], q[2]};
-           qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
+    double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
     double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
+    VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
     VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
+    VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
     VectorOperations<double>::add(sum1, sum2, sum, 3);
     VectorOperations<double>::subtract(sum, sum3, sum, 3);
     double omegaRefNew[3] = {0, 0, 0};
@@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
     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.
-    //		Does not make sense, but is implemented that way in MATLAB ?!
-    //		Thought: It does not really play a role, because in case there are more then one
-    //		reaction wheel invalid the pointing control is destined to fail.
     return returnvalue::FAILED;
   }
 }

mission/controller/acs/Guidance.h

@@ -15,7 +15,7 @@ class Guidance {
   void getTargetParamsSafe(double sunTargetSafe[3]);
   ReturnValue_t solarArrayDeploymentComplete();
 
-  // Function to get the target quaternion and refence rotation rate from gps position and
+  // Function to get the target quaternion and reference rotation rate from gps position and
   // position of the ground station
   void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
                                double refDirB[3], double quatBI[4], double targetQuat[4],
@@ -25,9 +25,10 @@ class Guidance {
   void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
                        double targetSatRotRate[3]);
 
-  // Function to get the target quaternion and refence rotation rate for sun pointing after ground
+  // Function to get the target quaternion and reference rotation rate for sun pointing after ground
   // station
-  void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                        double targetSatRotRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
@@ -42,10 +43,10 @@ class Guidance {
   // rate. Lastly gives back the error angle of the error quaternion.
   void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                   double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
-                  double errorQuat[4], double errorSatRotRate[3], double errorAngle);
+                  double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
   void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
-                  double errorAngle);
+                  double &errorAngle);
 
   void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                           double *targetSatRotRate);

mission/controller/acs/control/Detumble.cpp

@@ -7,8 +7,10 @@ Detumble::Detumble() {}
 
 Detumble::~Detumble() {}
 
-uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
+acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
-                                   const bool magFieldRateValid, const bool useFullDetumbleLaw) {
+                                                 const bool satRotRateValid,
+                                                 const bool magFieldRateValid,
+                                                 const bool useFullDetumbleLaw) {
   if (not magFieldValid) {
     return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (satRotRateValid and useFullDetumbleLaw) {

mission/controller/acs/control/Detumble.h

@@ -11,8 +11,9 @@ class Detumble {
   Detumble();
   virtual ~Detumble();
 
-  uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
+  acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                           const bool magFieldRateValid, const bool useFullDetumbleLaw);
+                                         const bool magFieldRateValid,
+                                         const bool useFullDetumbleLaw);
 
   void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
                    double gain);

mission/controller/acs/control/PtgCtrl.cpp

@@ -5,9 +5,6 @@
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/globalfunctions/sign.h>
-#include <math.h>
-
-#include "../util/MathOperations.h"
 
 PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
@@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qErrorLaw[3] = {0, 0, 0};
 
   for (int i = 0; i < 3; i++) {
-    if (abs(qError[i]) < qErrorMin) {
+    if (std::abs(qError[i]) < qErrorMin) {
       qErrorLaw[i] = qErrorMin;
     } else {
-      qErrorLaw[i] = abs(qError[i]);
+      qErrorLaw[i] = std::abs(qError[i]);
     }
   }
+
   double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
 
   double gain1 = cInt * omMax / qErrorLawNorm;
@@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double pErrorSign[3] = {0, 0, 0};
 
   for (int i = 0; i < 3; i++) {
-    if (abs(pError[i]) > 1) {
+    if (std::abs(pError[i]) > 1) {
       pErrorSign[i] = sign(pError[i]);
     } else {
       pErrorSign[i] = pError[i];
@@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
+void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                           const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                           const int32_t speedRw3, double *rwTrqNs) {
+  // 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)};
+  VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
+  VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
+
+  // calculate RPM offset utilizing the nullspace
+  double rpmOffset[4] = {0, 0, 0, 0};
+  double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
+  VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
+                                      rpmOffset, 4);
+
+  // calculate resulting angular momentum
+  double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
+  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
+  VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
+                                      rwAngMomentum, 4);
+
+  // calculate resulting torque
+  double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
+                                     acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
+                                     1, 4);
+  MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
+  VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
+                                      4);
+}
+
 void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
-                              double *magFieldEst, bool magFieldEstValid, double *satRate,
+                              const double *magFieldB, const bool magFieldBValid,
-                              int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
+                              const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
-                              int32_t *speedRw3, double *mgtDpDes) {
+                              const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
-  if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
+  if (not magFieldBValid or not pointingLawParameters->desatOn) {
-    mgtDpDes[0] = 0;
-    mgtDpDes[1] = 0;
-    mgtDpDes[2] = 0;
     return;
   }
 
-  // calculating momentum of satellite and momentum of reaction wheels
+  // concentrate RW speeds as vector and convert to double
-  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
+  double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
-  double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
+                        static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
-  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,
+  // convert magFieldB from uT to T
-                           const int32_t *speedRw0, const int32_t *speedRw1,
+  double magFieldBT[3] = {0, 0, 0};
-                           const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
-  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
+
-  double wheelMomentum[4] = {0, 0, 0, 0};
+  // calculate angular momentum of the satellite
-  double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
+  double angMomentumSat[3] = {0, 0, 0};
-  // conversion to [rad/s] for further calculations
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
-  VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
+                                     angMomentumSat, 3, 3, 1);
-  VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
+
-  double diffRwSpeed[4] = {0, 0, 0, 0};
+  // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
-  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
+  // relocate RW speed zero to nullspace RW speed
-  VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
+  double refSpeedRws[4] = {0, 0, 0, 0};
-                                      wheelMomentum, 4);
+  VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
-  double gainNs = pointingLawParameters->gainNullspace;
+                                      pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
-  double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
-  MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
+  // convert speed from 10 RPM to 1 RPM
-                                                acsParameters->rwMatrices.nullspace,
+  VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
-                                                *nullSpaceMatrix, 4);
+  // convert to rad/s
-  MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
+  VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
-  VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
+  // calculate angular momentum of each RW
-  VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
+  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) {
@@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
       if (rwCmdSpeeds[i] != 0) {
         if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
             rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
-          if (currRwSpeed[i] == 0) {
+          if (rwCmdSpeeds[i] > currRwSpeed[i]) {
-            if (rwCmdSpeeds[i] > 0) {
             rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
-            } else if (rwCmdSpeeds[i] < 0) {
+          } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
-              rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
-            }
-          } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
-            rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
-          } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
             rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
           }
         }

mission/controller/acs/control/PtgCtrl.h

@@ -1,13 +1,10 @@
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_
 
+#include <math.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
 #include <stdio.h>
-#include <string.h>
-#include <time.h>
-
-#include "../AcsParameters.h"
-#include "../SensorValues.h"
-#include "eive/resultClassIds.h"
 
 class PtgCtrl {
   /*
@@ -29,14 +26,14 @@ 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 speedRw0, const int32_t speedRw1, const int32_t speedRw2,
-                    const int32_t *speedRw3, double *rwTrqNs);
+                    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
@@ -45,6 +42,7 @@ class PtgCtrl {
 
  private:
   const AcsParameters *acsParameters;
+  static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
 };
 
 #endif /* ACS_CONTROL_PTGCTRL_H_ */

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                                  const bool satRotRateValid, const bool sunDirValid,
-                                   const uint8_t mekfEnabled, const uint8_t dampingEnabled) {
+                                                 const uint8_t mekfEnabled,
+                                                 const uint8_t dampingEnabled) {
   if (not magFieldValid) {
     return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {

mission/controller/acs/control/SafeCtrl.h

@@ -12,7 +12,7 @@ class SafeCtrl {
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
-  uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+  acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                          const bool satRotRateValid, const bool sunDirValid,
                                          const uint8_t mekfEnabled, const uint8_t dampingEnabled);