Merge remote-tracking branch 'origin/develop' into refactor_tm_handling

2023-03-10 19:35:45 +01:00 · 2023-03-10 19:35:45 +01:00 · 8c450596f6
commit 8c450596f6
parent 5ea2767662 b4129950cf
23 changed files with 474 additions and 485 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -16,10 +16,35 @@ will consitute of a breaking change warranting a new major release:
 # [unreleased]
 ## Added
 - `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out
  of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude
  set in the `AcsParameters`.
 - `AcsController` will now never command a RW speed larger than the maximum allowed speed.
 ## Fixed
 - `PAPB_EMPTY_SIGNAL_VC1` GPIO was not set up properly.
 - Fix for heater names: HPA heater (index 7) is now the Syrlinks heater.
 - `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix
  parameters use their respective setters.
 - Several `AcsController` components had their own implementation of `AcsParameters`. This resulted
  in those parameters not being updated, while the actual ones were updated. All instances of
  `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer
  instances.
 - Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters`
  were updated.
 - Instead of updating the `idleModeControllerParameters`, the `targetModeControllerParameters`
  were updated.
 - Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control
  values.
 - Fixed wrong check on wether file used for persistant boolean flag on successful still existed.
 - Scaling of MTQ Cmds now scales the current values to command with the current values and not
  the values of the last step, which would result in undefined behaviour.
 - Solved naming collision between file used for solar array deployment and confirmation for
  ACS for solar array deployment.
 - Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded.
 ## Changed
@ -31,6 +56,9 @@ will consitute of a breaking change warranting a new major release:
 - Service 5 now handles 40 events per cycle instead of 15
 - Remove periodic SD card check. The file system is not mounted read-only anymore when using an
  ext4 filesystem
 - The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get
  violated anymore. Instead it is incrementally reset.
 - The RW antistiction now only takes the RW speeds in account.
 - ACS CTRL transition to DETUBMLE is now done in CTRL internally. No
  system level handling necessary anymore.
 - More fixes and improvements for SD card handling. Extend SD card setup in core controller to
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -6,12 +6,8 @@
 AcsController::AcsController(object_id_t objectId)
    : ExtendedControllerBase(objectId),
      sensorProcessing(&acsParameters),
      navigation(&acsParameters),
      actuatorCmd(&acsParameters),
      guidance(&acsParameters),
      safeCtrl(&acsParameters),
      detumble(&acsParameters),
      ptgCtrl(&acsParameters),
      parameterHelper(this),
      mgmDataRaw(this),
@ -146,7 +142,7 @@ void AcsController::performSafe() {
  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
    if (not mekfInvalidFlag) {
@ -180,7 +176,9 @@ void AcsController::performSafe() {
    // ToDo: this should never ever happen or we are dead. prob add an event at least
  }
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
+  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
                          *acsParameters.magnetorquerParameter.inverseAlignment,
                          acsParameters.magnetorquerParameter.dipolMax);
  // detumble check and switch
  if (mekfData.satRotRateMekf.isValid() &&
@ -191,8 +189,8 @@ void AcsController::performSafe() {
             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
                 acsParameters.detumbleParameter.omegaDetumbleStart) {
    detumbleCounter++;
-  } else {
+  } else if (detumbleCounter > 0) {
-    detumbleCounter = 0;
+    detumbleCounter -= 1;
  }
  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    detumbleCounter = 0;
@ -203,7 +201,7 @@ void AcsController::performSafe() {
  updateCtrlValData(errAng);
  updateActuatorCmdData(cmdDipolMtqs);
-  // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
+  // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
  //                  acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
  //                  acsParameters.rwHandlingParameters.rampTime);
 }
@ -215,7 +213,7 @@ void AcsController::performDetumble() {
  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
    if (not mekfInvalidFlag) {
@ -228,8 +226,11 @@ void AcsController::performDetumble() {
  double magMomMtq[3] = {0, 0, 0};
  detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                   mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
-                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
+                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
+                   acsParameters.detumbleParameter.gainD);
  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
                          *acsParameters.magnetorquerParameter.inverseAlignment,
                          acsParameters.magnetorquerParameter.dipolMax);
  if (mekfData.satRotRateMekf.isValid() &&
      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -239,8 +240,8 @@ void AcsController::performDetumble() {
             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
-  } else {
+  } else if (detumbleCounter > 0) {
-    detumbleCounter = 0;
+    detumbleCounter -= 1;
  }
  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    detumbleCounter = 0;
@ -263,7 +264,7 @@ void AcsController::performPointingCtrl() {
  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                                            &susDataProcessed, &mekfData);
+                                            &susDataProcessed, &mekfData, &acsParameters);
  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
    if (not mekfInvalidFlag) {
@ -296,24 +297,26 @@ void AcsController::performPointingCtrl() {
  } else {
    multipleRwUnavailableCounter = 0;
  }
  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};
  // Variables required for guidance
  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
         errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
  // 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 (submode) {
    case acs::PTG_IDLE:
      guidance.targetQuatPtgSun(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(
          &acsParameters.idleModeControllerParameters, &(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);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -337,7 +340,7 @@ void AcsController::performPointingCtrl() {
          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -351,20 +354,20 @@ void AcsController::performPointingCtrl() {
                               susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
+      ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &acsParameters.gsTargetModeControllerParameters, &(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);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
-          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          &acsParameters.gsTargetModeControllerParameters, 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);
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
      break;
    case acs::PTG_NADIR:
@ -382,7 +385,7 @@ void AcsController::performPointingCtrl() {
          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -405,7 +408,7 @@ void AcsController::performPointingCtrl() {
          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -415,18 +418,21 @@ void AcsController::performPointingCtrl() {
      break;
  }
  actuatorCmd.cmdSpeedToRws(
      sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
      cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
      acsParameters.rwHandlingParameters.maxRwSpeed,
      acsParameters.rwHandlingParameters.inertiaWheel);
  if (enableAntiStiction) {
-    ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
+    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
  }
-
+  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
-  actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
+                          *acsParameters.magnetorquerParameter.inverseAlignment,
-                            sensorValues.rw2Set.currSpeed.value,
+                          acsParameters.magnetorquerParameter.dipolMax);
                            sensorValues.rw3Set.currSpeed.value,
                            sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
-  updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
+  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
  //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
  //                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -584,6 +590,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
  // GPS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@ -190,6 +190,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
  acsctrl::GpsDataProcessed gpsDataProcessed;
  PoolEntry<double> gcLatitude = PoolEntry<double>();
  PoolEntry<double> gdLongitude = PoolEntry<double>();
  PoolEntry<double> altitude = PoolEntry<double>();
  PoolEntry<double> gpsPosition = PoolEntry<double>(3);
  PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case 0x2:  // InertiaEIVE
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(inertiaEIVE.inertiaMatrix);
+          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
+          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed);
          break;
        case 0x2:
-          parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
+          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed);
          break;
        case 0x3:
-          parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
+          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1);
          break;
        case 0x4:
-          parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
+          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case 0x3:  // MgmHandlingParameters
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix);
          break;
        case 0x2:
-          parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix);
          break;
        case 0x3:
-          parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix);
          break;
        case 0x4:
-          parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix);
          break;
        case 0x5:
-          parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset);
          break;
        case 0x6:
-          parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset);
          break;
        case 0x7:
-          parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset);
          break;
        case 0x8:
-          parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset);
          break;
        case 0x9:
-          parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset);
          break;
        case 0xA:
-          parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse);
          break;
        case 0xB:
-          parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse);
          break;
        case 0xC:
-          parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse);
          break;
        case 0xD:
-          parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse);
          break;
        case 0xE:
-          parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
+          parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse);
          break;
        case 0xF:
-          parameterWrapper->set(mgmHandlingParameters.mgm02variance);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm02variance);
          break;
        case 0x10:
-          parameterWrapper->set(mgmHandlingParameters.mgm13variance);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm13variance);
          break;
        case 0x11:
-          parameterWrapper->set(mgmHandlingParameters.mgm4variance);
+          parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case 0x4:  // SusHandlingParameters
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix);
          break;
        case 0x2:
-          parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix);
          break;
        case 0x3:
-          parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix);
          break;
        case 0x4:
-          parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix);
          break;
        case 0x5:
-          parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix);
          break;
        case 0x6:
-          parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix);
          break;
        case 0x7:
-          parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix);
          break;
        case 0x8:
-          parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix);
          break;
        case 0x9:
-          parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix);
          break;
        case 0xA:
-          parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix);
          break;
        case 0xB:
-          parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
+          parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix);
          break;
        case 0xC:
-          parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha);
          break;
        case 0xD:
-          parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta);
          break;
        case 0xE:
-          parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha);
          break;
        case 0xF:
-          parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta);
          break;
        case 0x10:
-          parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha);
          break;
        case 0x11:
-          parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta);
          break;
        case 0x12:
-          parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha);
          break;
        case 0x13:
-          parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta);
          break;
        case 0x14:
-          parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha);
          break;
        case 0x15:
-          parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta);
          break;
        case 0x16:
-          parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha);
          break;
        case 0x17:
-          parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta);
          break;
        case 0x18:
-          parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha);
          break;
        case 0x19:
-          parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta);
          break;
        case 0x1A:
-          parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha);
          break;
        case 0x1B:
-          parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta);
          break;
        case 0x1C:
-          parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha);
          break;
        case 0x1D:
-          parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta);
          break;
        case 0x1E:
-          parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha);
          break;
        case 0x1F:
-          parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta);
          break;
        case 0x20:
-          parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha);
          break;
        case 0x21:
-          parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta);
          break;
        case 0x22:
-          parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
+          parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha);
          break;
        case 0x23:
-          parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
+          parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case (0x5):  // GyrHandlingParameters
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
+          parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
+          parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix);
          break;
        case 0x2:
-          parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
+          parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix);
          break;
        case 0x3:
-          parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
+          parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix);
          break;
        case 0x4:
-          parameterWrapper->set(gyrHandlingParameters.gyr0bias);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr0bias);
          break;
        case 0x5:
-          parameterWrapper->set(gyrHandlingParameters.gyr1bias);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr1bias);
          break;
        case 0x6:
-          parameterWrapper->set(gyrHandlingParameters.gyr2bias);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr2bias);
          break;
        case 0x7:
-          parameterWrapper->set(gyrHandlingParameters.gyr3bias);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr3bias);
          break;
        case 0x8:
-          parameterWrapper->set(gyrHandlingParameters.gyr02variance);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr02variance);
          break;
        case 0x9:
-          parameterWrapper->set(gyrHandlingParameters.gyr13variance);
+          parameterWrapper->setVector(gyrHandlingParameters.gyr13variance);
          break;
        case 0xA:
          parameterWrapper->set(gyrHandlingParameters.preferAdis);
@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(rwHandlingParameters.maxTrq);
          break;
        case 0x2:
-          parameterWrapper->set(rwHandlingParameters.stictionSpeed);
+          parameterWrapper->set(rwHandlingParameters.maxRwSpeed);
          break;
        case 0x3:
-          parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
+          parameterWrapper->set(rwHandlingParameters.stictionSpeed);
          break;
        case 0x4:
-          parameterWrapper->set(rwHandlingParameters.stictionTorque);
+          parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
          break;
        case 0x5:
          parameterWrapper->set(rwHandlingParameters.stictionTorque);
          break;
        case 0x6:
          parameterWrapper->set(rwHandlingParameters.rampTime);
          break;
        default:
@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case (0x7):  // RwMatrices
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(rwMatrices.alignmentMatrix);
+          parameterWrapper->setMatrix(rwMatrices.alignmentMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(rwMatrices.pseudoInverse);
+          parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
          break;
        case 0x2:
-          parameterWrapper->set(rwMatrices.without1);
+          parameterWrapper->setMatrix(rwMatrices.without1);
          break;
        case 0x3:
-          parameterWrapper->set(rwMatrices.without2);
+          parameterWrapper->setMatrix(rwMatrices.without2);
          break;
        case 0x4:
-          parameterWrapper->set(rwMatrices.without3);
+          parameterWrapper->setMatrix(rwMatrices.without3);
          break;
        case 0x5:
-          parameterWrapper->set(rwMatrices.without4);
+          parameterWrapper->setMatrix(rwMatrices.without4);
          break;
        case 0x6:
-          parameterWrapper->set(rwMatrices.nullspace);
+          parameterWrapper->setVector(rwMatrices.nullspace);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
          break;
        case 0x5:
-          parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop);
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
          break;
        case 0x6:
-          parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
          break;
        case 0x7:
-          parameterWrapper->set(safeModeControllerParameters.satRateRef);
+          parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case (0x9):  // IdleModeControllerParameters
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(targetModeControllerParameters.zeta);
+          parameterWrapper->set(idleModeControllerParameters.zeta);
          break;
        case 0x1:
-          parameterWrapper->set(targetModeControllerParameters.om);
+          parameterWrapper->set(idleModeControllerParameters.om);
          break;
        case 0x2:
-          parameterWrapper->set(targetModeControllerParameters.omMax);
+          parameterWrapper->set(idleModeControllerParameters.omMax);
          break;
        case 0x3:
-          parameterWrapper->set(targetModeControllerParameters.qiMin);
+          parameterWrapper->set(idleModeControllerParameters.qiMin);
          break;
        case 0x4:
-          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
+          parameterWrapper->set(idleModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
-          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
-          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          parameterWrapper->set(idleModeControllerParameters.desatOn);
          break;
        case 0x8:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
          break;
        default:
@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
-          parameterWrapper->set(targetModeControllerParameters.refDirection);
+          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
          break;
        case 0xA:
-          parameterWrapper->set(targetModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
          break;
        case 0xB:
-          parameterWrapper->set(targetModeControllerParameters.quatRef);
+          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
          break;
        case 0xC:
          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case (0xB):  // GsTargetModeControllerParameters
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(targetModeControllerParameters.zeta);
+          parameterWrapper->set(gsTargetModeControllerParameters.zeta);
          break;
        case 0x1:
-          parameterWrapper->set(targetModeControllerParameters.om);
+          parameterWrapper->set(gsTargetModeControllerParameters.om);
          break;
        case 0x2:
-          parameterWrapper->set(targetModeControllerParameters.omMax);
+          parameterWrapper->set(gsTargetModeControllerParameters.omMax);
          break;
        case 0x3:
-          parameterWrapper->set(targetModeControllerParameters.qiMin);
+          parameterWrapper->set(gsTargetModeControllerParameters.qiMin);
          break;
        case 0x4:
-          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
+          parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
-          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
-          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
          break;
        case 0x8:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
-          parameterWrapper->set(targetModeControllerParameters.refDirection);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
          break;
        case 0xA:
-          parameterWrapper->set(targetModeControllerParameters.refRotRate);
+          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
          break;
        case 0xB:
-          parameterWrapper->set(targetModeControllerParameters.quatRef);
+          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
          break;
        case 0xC:
-          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
          break;
        case 0xD:
-          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
          break;
        case 0xE:
          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
          break;
        case 0xF:
          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef);
+          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
-          parameterWrapper->set(nadirModeControllerParameters.refDirection);
+          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
          break;
        case 0xA:
-          parameterWrapper->set(nadirModeControllerParameters.quatRef);
+          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
          break;
        case 0xC:
          parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef);
+          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
-          parameterWrapper->set(inertialModeControllerParameters.tgtQuat);
+          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
          break;
        case 0xA:
-          parameterWrapper->set(inertialModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
          break;
        case 0xC:
-          parameterWrapper->set(inertialModeControllerParameters.quatRef);
+          parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(strParameters.exclusionAngle);
          break;
        case 0x1:
-          parameterWrapper->set(strParameters.boresightAxis);
+          parameterWrapper->setVector(strParameters.boresightAxis);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
        case 0x0:
          parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
          break;
        case 0x1:
          parameterWrapper->set(gpsParameters.minimumFdirAltitude);
          break;
        case 0x2:
          parameterWrapper->set(gpsParameters.maximumFdirAltitude);
          break;
        case 0x3:
          parameterWrapper->set(gpsParameters.fdirAltitude);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
    case (0x12):  // MagnetorquesParameter
      switch (parameterId) {
        case 0x0:
-          parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
+          parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix);
          break;
        case 0x1:
-          parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
+          parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix);
          break;
        case 0x2:
-          parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
+          parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix);
          break;
        case 0x3:
-          parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
+          parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq);
          break;
        case 0x4:
-          parameterWrapper->set(magnetorquesParameter.inverseAlignment);
+          parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
          break;
        case 0x5:
-          parameterWrapper->set(magnetorquesParameter.DipolMax);
+          parameterWrapper->set(magnetorquerParameter.dipolMax);
          break;
        case 0x6:
-          parameterWrapper->set(magnetorquesParameter.torqueDuration);
+          parameterWrapper->set(magnetorquerParameter.torqueDuration);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF {
    double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
    double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
-    /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
+    /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
     * assumed to be equal for the same class of sensors */
    float gyr02variance[3] = {pow(3.0e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
                              pow(3.0e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
@ -784,8 +784,9 @@ class AcsParameters : public HasParametersIF {
  struct RwHandlingParameters {
    double inertiaWheel = 0.000028198;
    double maxTrq = 0.0032;               // 3.2 [mNm]
-    int32_t stictionSpeed = 100;         // RPM
+    int32_t maxRwSpeed = 65000;           // 0.1 RPM
-    int32_t stictionReleaseSpeed = 120;  // RPM
+    int32_t stictionSpeed = 1000;         // 0.1 RPM
    int32_t stictionReleaseSpeed = 1000;  // 0.1 RPM
    double stictionTorque = 0.0006;
    uint16_t rampTime = 10;
@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF {
    double sunMagAngleMin = 5 * M_PI / 180;
-    double sunTargetDirLeop[3] = {0, .5, .5};
+    double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
    double sunTargetDir[3] = {0, 0, 1};
    double satRateRef[3] = {0, 0, 0};
@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF {
    double refDirection[3] = {-1, 0, 0};  // Antenna
    double refRotRate[3] = {0, 0, 0};
    double quatRef[4] = {0, 0, 0, 1};
-    int8_t timeElapsedMax = 10;  // rot rate calculations
+    uint8_t timeElapsedMax = 10;  // rot rate calculations
    // Default is Stuttgart GS
    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF {
  struct GsTargetModeControllerParameters : PointingLawParameters {
    double refDirection[3] = {-1, 0, 0};  // Antenna
-    int8_t timeElapsedMax = 10;           // rot rate calculations
+    uint8_t timeElapsedMax = 10;          // rot rate calculations
    // Default is Stuttgart GS
    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
@ -871,7 +872,7 @@ class AcsParameters : public HasParametersIF {
    double refDirection[3] = {-1, 0, 0};  // Antenna
    double quatRef[4] = {0, 0, 0, 1};
    double refRotRate[3] = {0, 0, 0};
-    int8_t timeElapsedMax = 10;  // rot rate calculations
+    uint8_t timeElapsedMax = 10;  // rot rate calculations
  } nadirModeControllerParameters;
  struct InertialModeControllerParameters : PointingLawParameters {
@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF {
  } strParameters;
  struct GpsParameters {
-    double timeDiffVelocityMax = 30;  //[s]
+    double timeDiffVelocityMax = 30;         // [s]
    double minimumFdirAltitude = 475 * 1e3;  // [m]
    double maximumFdirAltitude = 575 * 1e3;  // [m]
    double fdirAltitude = 525 * 1e3;         // [m]
  } gpsParameters;
  struct SunModelParameters {
@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF {
    double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600;                // Bias Stability
  } kalmanFilterParameters;
-  struct MagnetorquesParameter {
+  struct MagnetorquerParameter {
    double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};
    double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
    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 dipolMax = 0.2;  // [Am^2]
    uint16_t torqueDuration = 300;  // [ms]
-  } magnetorquesParameter;
+  } magnetorquerParameter;
  struct DetumbleParameter {
    uint8_t detumblecounter = 75;  // 30 s
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@ -10,64 +10,60 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
-ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
+ActuatorCmd::ActuatorCmd() {}
 ActuatorCmd::~ActuatorCmd() {}
-void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
+void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
-  // Scaling the commanded torque to a maximum value
+  uint8_t maxIdx = 0;
-  double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
+  VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
  double maxValue = rwTrq[maxIdx];
-  double maxValue = 0;
+  if (maxValue > maxTorque) {
-  for (int i = 0; i < 4; i++) {  // size of torque, always 4 ?
+    double scalingFactor = maxTorque / maxValue;
-    if (abs(rwTrq[i]) > maxValue) {
+    VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
      maxValue = abs(rwTrq[i]);
    }
  }
  if (maxValue > maxTrq) {
    double scalingFactor = maxTrq / maxValue;
    VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
  }
 }
-void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
+void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
-                                const int32_t speedRw2, const int32_t speedRw3,
+                                int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
-                                const double *rwTorque, int32_t *rwCmdSpeed) {
+                                double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
  using namespace Math;
  // Calculating the commanded speed in RPM for every reaction wheel
  int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
  double deltaSpeed[4] = {0, 0, 0, 0};
  double commandTime = acsParameters.onBoardParams.sampleTime,
         inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
-  double factor = commandTime / inertiaWheel * radToRpm;
+  double factor = sampleTime / inertiaWheel * radToRpm;
  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
  for (int i = 0; i < 4; i++) {
    deltaSpeedInt[i] = std::round(deltaSpeed[i]);
  }
  VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
  for (uint8_t i = 0; i < 4; i++) {
    if (rwCmdSpeed[i] > maxRwSpeed) {
      rwCmdSpeed[i] = maxRwSpeed;
    } else if (rwCmdSpeed[i] < -maxRwSpeed) {
      rwCmdSpeed[i] = -maxRwSpeed;
    }
  }
  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
 }
-void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
+void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
                              const double *inverseAlignment, double maxDipol) {
  // Convert to actuator frame
  double dipolMomentActuatorDouble[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
+  MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
-                                     dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
+                                     1);
  // Scaling along largest element if dipol exceeds maximum
-  double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
+  uint8_t maxIdx = 0;
-  double maxValue = 0;
+  VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
-  for (int i = 0; i < 3; i++) {
+  double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
-    if (abs(dipolMomentActuator[i]) > maxDipol) {
+  if (maxAbsValue > maxDipol) {
-      maxValue = abs(dipolMomentActuator[i]);
+    double scalingFactor = maxDipol / maxAbsValue;
    }
  }
  if (maxValue > maxDipol) {
    double scalingFactor = maxDipol / maxValue;
    VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
                                        dipolMomentActuatorDouble, 3);
  }
--- a/mission/controller/acs/ActuatorCmd.h
+++ b/mission/controller/acs/ActuatorCmd.h
@ -1,23 +1,22 @@
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_
 #include "AcsParameters.h"
 #include "MultiplicativeKalmanFilter.h"
 #include "SensorProcessing.h"
 #include "SensorValues.h"
 class ActuatorCmd {
 public:
-  ActuatorCmd(AcsParameters *acsParameters_);  // Input mode ?
+  ActuatorCmd();
  virtual ~ActuatorCmd();
  /*
   * @brief:	scalingTorqueRws()	scales the torque via maximum part in case this part is
   * higher then the maximum torque
-   * @param:	rwTrq	 			given torque for reaction wheels
+   * @param:	rwTrq	 			given torque for reaction wheels which will be
-   * 			rwTrqScaled			possible scaled torque
+   * scaled if needed to be
   */
-  void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
+  void scalingTorqueRws(double *rwTrq, double maxTorque);
  /*
   * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
@ -28,8 +27,9 @@ class ActuatorCmd {
   * 			rwCmdSpeed			output revolutions per minute for every
   * reaction wheel
   */
-  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+  void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
-                     const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
+                     const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
                     int32_t maxRwSpeed, double inertiaWheel);
  /*
   * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
@ -37,11 +37,11 @@ class ActuatorCmd {
   * @param:	dipolMoment 		given dipol moment in spacecraft frame
   * 			dipolMomentActuator	resulting dipol moment in actuator reference frame
   */
-  void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
+  void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
                   const double *inverseAlignment, double maxDipol);
 protected:
 private:
  AcsParameters acsParameters;
 };
 #endif /* ACTUATORCMD_H_ */
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@ -12,7 +12,7 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
-Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
+Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 Guidance::~Guidance() {}
@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
  double targetE[3] = {0, 0, 0};
  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters->targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
  // target direction in the ECEF frame
  double targetDirE[3] = {0, 0, 0};
@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
  // rotation quaternion from two vectors
  double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
+  refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
+  refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
+  refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
  double noramlizedTargetDirB[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
  VectorOperations<double>::normalize(refDir, refDir, 3);
@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate in case of star tracker blinding
  //-------------------------------------------------------------------------------------
-  if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
+  if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
    double sunDirB[3] = {0, 0, 0};
    MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
-    double exclAngle = acsParameters.strParameters.exclusionAngle,
+    double exclAngle = acsParameters->strParameters.exclusionAngle,
-           blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
+           blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
-           blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
+           blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
    double sightAngleSun =
-        VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
+        VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
    if (!(strBlindAvoidFlag)) {
      double critSightAngle = blindStart * exclAngle;
@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
      }
    } else {
      if (sightAngleSun < blindEnd * exclAngle) {
-        double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
+        double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
        double blindRefRate[3] = {0, 0, 0};
        if (sunDirB[1] < 0) {
          blindRefRate[0] = normBlindRefRate;
@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double targetE[3] = {0, 0, 0};
  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters->targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
@ -198,7 +198,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
                        {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);
-  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
@ -211,9 +211,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
  double groundStationE[3] = {0, 0, 0};
  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.gsTargetModeControllerParameters.latitudeTgt,
+      acsParameters->gsTargetModeControllerParameters.latitudeTgt,
-      acsParameters.gsTargetModeControllerParameters.longitudeTgt,
+      acsParameters->gsTargetModeControllerParameters.longitudeTgt,
-      acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
+      acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
@ -262,7 +262,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
                         {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
-  int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
  // rotation quaternion from two vectors
  double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
+  refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
+  refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
+  refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
  double noramlizedTargetDirB[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
  VectorOperations<double>::normalize(refDir, refDir, 3);
@ -406,7 +406,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
                         {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
-  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
 }
@ -516,19 +516,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
  bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
  if (rw1valid && rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
    return returnvalue::OK;
  } else {
    // 	    @note: This one takes the normal pseudoInverse of all four raction wheels valid.
@ -542,15 +542,14 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
  std::error_code e;
  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
-      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE,
+      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
-                                  e)) {  // ToDo: if file does not exist anymore
+    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
                3 * sizeof(double));
  } else {
-    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
+    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
                3 * sizeof(double));
  }
-  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
+  std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
              3 * sizeof(double));
 }
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@ -55,14 +55,15 @@ class Guidance {
  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 private:
-  AcsParameters acsParameters;
+  const AcsParameters *acsParameters;
  bool strBlindAvoidFlag = false;
  timeval timeSavedQuaternion;
  double savedQuaternion[4] = {0, 0, 0, 0};
  double omegaRefSaved[3] = {0, 0, 0};
-  static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
+  static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
-  static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
+  static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
 };
 #endif /* ACS_GUIDANCE_H_ */
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@ -12,33 +12,22 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
-/*Initialisation of values for parameters in constructor*/
+MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
 MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
    : initialQuaternion{0, 0, 0, 1},
      initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                              {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
  loadAcsParameters(acsParameters_);
 }
 MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
 void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
  inertiaEIVE = &(acsParameters_->inertiaEIVE);
  kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 ReturnValue_t MultiplicativeKalmanFilter::init(
    const double *magneticField_, const bool validMagField_, const double *sunDir_,
    const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-    const bool validMagModel, acsctrl::MekfData *mekfData) {  // valids for "model measurements"?
+    const bool validMagModel, acsctrl::MekfData *mekfData,
    AcsParameters *acsParameters) {  // valids for "model measurements"?
  // check for valid mag/sun
  if (validMagField_ && validSS && validSSModel && validMagModel) {
    validInit = true;
    //  QUEST ALGO -----------------------------------------------------------------------
    double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
-    sigmaSun = kalmanFilterParameters->sensorNoiseSS;
+    sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
-    sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
+    sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
-    sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
+    sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
    double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
           normSunJ[3] = {0, 0, 0};
@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
    return MEKF_INITIALIZED;
  } else {
    // no initialisation possible, no valid measurements
    validInit = false;
    updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
    return MEKF_UNINITIALIZED;
  }
 }
 // --------------- MEKF DISCRETE TIME STEP -------------------------------
-ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
+ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
-                                                  const double *rateGYRs_, const bool validGYRs_,
+    const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
-                                                  const double *magneticField_,
+    const bool validGYRs_, const double *magneticField_, const bool validMagField_,
-                                                  const bool validMagField_, const double *sunDir_,
+    const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
-                                                  const bool validSS, const double *sunDirJ,
+    const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
-                                                  const bool validSSModel, const double *magFieldJ,
+    AcsParameters *acsParameters) {
                                                  const bool validMagModel, double sampleTime,
                                                  acsctrl::MekfData *mekfData) {
  // Check for GYR Measurements
  int MDF = 0;  // Matrix Dimension Factor
  if (!validGYRs_) {
@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
  // If we are here, MEKF will perform
  double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
-  sigmaSun = kalmanFilterParameters->sensorNoiseSS;
+  sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
-  sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
+  sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
-  sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
+  sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
  double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
         normSunJ[3] = {0, 0, 0};
@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
  biasGYR[2] = updatedGyroBias[2];
  /* ----------- PROPAGATION ----------*/
-  double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
+  double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
-  double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
+  double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
  double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
                                 {0, 0, 0, 1, 0, 0},  {0, 0, 0, 0, 1, 0},  {0, 0, 0, 0, 0, 1}};
@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
         covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  if (normRotEst * sampleTime < M_PI / 10) {
+  if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
-    double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
+    double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime +
                    1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3);
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
-    double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
+    double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2));
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
    std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
-    double fact22 = pow(sigmaU, 2) * sampleTime;
+    double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
  } else {
-    double fact22 = pow(sigmaU, 2) * sampleTime;
+    double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
    double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
-    double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
+    double fact12_0 =
        (normRotEst * acsParameters->onBoardParams.sampleTime -
         sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
    MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
-    double fact12_1 = 1. / 2. * pow(sampleTime, 2);
+    double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2);
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
    double fact12_2 =
-        (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
+        (1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) +
         cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) /
        pow(normRotEst, 4);
    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
    MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
@ -961,12 +951,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
    MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
    double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
-    double fact11_0 = pow(sigmaV, 2) * sampleTime;
+    double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime;
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
-    double fact11_1 = 1. / 3. * pow(sampleTime, 3);
+    double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3);
    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
-    double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
+    double fact11_2 =
-                       1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
+        (2 * normRotEst * acsParameters->onBoardParams.sampleTime -
         2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) -
         1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) /
        pow(normRotEst, 5);
    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
    MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
         phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                      {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
  double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
-  double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
+  double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
-  double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
+  double fact11_2 =
      (1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2);
  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
  MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
  MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
  double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
  double fact12_0 = fact11_2;
  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
+  MatrixOperations<double>::multiplyScalar(*identityMatrix3,
-  double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
+                                           acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3);
  double fact12_2 =
      (normRotEst * acsParameters->onBoardParams.sampleTime -
       sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
  MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
  MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
  // Propagated Quaternion
  double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double rotCos = cos(0.5 * normRotEst * sampleTime);
+  double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
-  double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
+  double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
  VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
  double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter {
   */
 public:
  /* @brief: Constructor
   * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
   */
-  MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
+  MultiplicativeKalmanFilter();
  virtual ~MultiplicativeKalmanFilter();
  ReturnValue_t reset(acsctrl::MekfData *mekfData);
@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter {
   */
  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
                     const bool validSS, const double *sunDirJ, const bool validSSModel,
-                     const double *magFieldJ, const bool validMagModel,
+                     const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
-                     acsctrl::MekfData *mekfData);
+                     AcsParameters *acsParameters);
  /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
   * for the current step after the initalization
@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter {
                        const bool validGYRs_, const double *magneticField_,
                        const bool validMagField_, const double *sunDir_, const bool validSS,
                        const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
+                        const bool validMagModel, acsctrl::MekfData *mekfData,
                        AcsParameters *acsParameters);
  enum MekfStatus : uint8_t {
    UNINITIALIZED = 0,
@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter {
 private:
  /*Parameters*/
  AcsParameters::InertiaEIVE *inertiaEIVE;
  AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
  double quaternion_STR_SB[4];
  bool validInit;
  /*States*/
-  double initialQuaternion[4];          /*after reset?QUEST*/
+  double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
-  double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
+  double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
  double propagatedQuaternion[4];       /*Filter Quaternion for next step*/
-  uint8_t sensorsAvail;
+  uint8_t sensorsAvail = 0;
  /*Outputs*/
  double quatBJ[4];  /* Output Quaternion */
  double biasGYR[3]; /*Between measured and estimated sat Rate*/
                     /*Parameter INIT*/
  /*Functions*/
  void loadAcsParameters(AcsParameters *acsParameters_);
  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
                     double satRotRate[3]);
--- a/mission/controller/acs/Navigation.cpp
+++ b/mission/controller/acs/Navigation.cpp
@ -8,9 +8,7 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
-Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
+Navigation::Navigation() {}
  acsParameters = *acsParameters_;
 }
 Navigation::~Navigation() {}
@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
                                  acsctrl::SusDataProcessed *susDataProcessed,
-                                  acsctrl::MekfData *mekfData) {
+                                  acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
  bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
+        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
        acsParameters);
    return mekfStatus;
  } else {
    mekfStatus = multiplicativeKalmanFilter.mekfEst(
@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
        mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
        susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
        susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
+        mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
    return mekfStatus;
  }
 }
--- a/mission/controller/acs/Navigation.h
+++ b/mission/controller/acs/Navigation.h
@ -9,19 +9,19 @@
 class Navigation {
 public:
-  Navigation(AcsParameters *acsParameters_);
+  Navigation();
  virtual ~Navigation();
  ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
                        acsctrl::GyrDataProcessed *gyrDataProcessed,
                        acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
+                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
                        AcsParameters *acsParameters);
  void resetMekf(acsctrl::MekfData *mekfData);
 protected:
 private:
  MultiplicativeKalmanFilter multiplicativeKalmanFilter;
  AcsParameters acsParameters;
  ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
 };
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@ -14,7 +14,7 @@
 using namespace Math;
-SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
+SensorProcessing::SensorProcessing() {}
 SensorProcessing::~SensorProcessing() {}
@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
                                  acsctrl::GpsDataProcessed *gpsDataProcessed,
                                  const double gpsAltitude, bool gpsValid,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed) {
-  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
+  // ---------------- IGRF- 13 Implementation here
  // ------------------------------------------------
  double magIgrfModel[3] = {0.0, 0.0, 0.0};
  if (gpsValid) {
    // Should be existing class object which will be called and modified here.
@ -549,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
                                  const bool validGps,
                                  const AcsParameters::GpsParameters *gpsParameters,
                                  acsctrl::GpsDataProcessed *gpsDataProcessed) {
-  // name to convert not process
+  double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
-  double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
+         gpsVelocityE[3] = {0, 0, 0};
  if (validGps) {
    // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
    gdLongitude = gpsLongitude * PI / 180.;
@ -559,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
    double factor = 1 - pow(eccentricityWgs84, 2);
    gcLatitude = atan(factor * tan(latitudeRad));
    // Altitude FDIR
    if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
        gpsAltitude < gpsParameters->maximumFdirAltitude) {
      altitude = gpsParameters->fdirAltitude;
    } else {
      altitude = gpsAltitude;
    }
    // Calculation of the satellite velocity in earth fixed frame
    double deltaDistance[3] = {0, 0, 0};
-    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
+    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
    if (validSavedPosSatE &&
        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
      VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
@ -580,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
    if (pg.getReadResult() == returnvalue::OK) {
      gpsDataProcessed->gdLongitude.value = gdLongitude;
      gpsDataProcessed->gcLatitude.value = gcLatitude;
      gpsDataProcessed->altitude.value = altitude;
      std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
      std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
      gpsDataProcessed->setValidity(validGps, true);
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@ -15,7 +15,7 @@ class SensorProcessing {
 public:
  void reset();
-  SensorProcessing(AcsParameters *acsParameters_);
+  SensorProcessing();
  virtual ~SensorProcessing();
  void process(timeval now, ACS::SensorValues *sensorValues,
@ -77,7 +77,6 @@ class SensorProcessing {
  bool validSavedPosSatE = false;
  SusConverter susConverter;
  AcsParameters acsParameters;
 };
 #endif /*SENSORPROCESSING_H_*/
--- a/mission/controller/acs/SensorValues.h
+++ b/mission/controller/acs/SensorValues.h
@ -1,17 +1,16 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 #include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
 #include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
 #include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <mission/devices/devicedefinitions/GPSDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 #include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
 #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
 namespace ACS {
 class SensorValues {
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@ -9,39 +9,32 @@
 #include "../util/MathOperations.h"
-Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
+Detumble::Detumble() {}
 Detumble::~Detumble() {}
 void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
  detumbleParameter = &(acsParameters_->detumbleParameter);
  magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
 }
 ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
-                                const double *magField, const bool magFieldValid, double *magMom) {
+                                const double *magField, const bool magFieldValid, double *magMom,
                                double gain) {
  if (!magRateValid || !magFieldValid) {
    return DETUMBLE_NO_SENSORDATA;
  }
-
+  // convert uT to T
-  // change unit from uT to T
+  double magFieldT[3], magRateT[3];
  double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
  VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
-
+  // control law
-  double gain = detumbleParameter->gainD;
+  double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
-  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
+  VectorOperations<double>::mulScalar(magRateT, factor, magMom, 3);
  VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
  return returnvalue::OK;
 }
-ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
+ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
-                                    double *magMom) {
+                                    double dipolMax) {
  if (!magRateValid) {
    return DETUMBLE_NO_SENSORDATA;
  }
  double dipolMax = magnetorquesParameter->DipolMax;
  for (int i = 0; i < 3; i++) {
    magMom[i] = -dipolMax * sign(magRate[i]);
  }
@ -49,14 +42,20 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
  return returnvalue::OK;
 }
-ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
+ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid,
                                    const double *magField, const bool *magFieldValid,
-                                    double *magMom) {
+                                    double *magMom, double gain) {
  if (!satRateValid || !magFieldValid) {
    return DETUMBLE_NO_SENSORDATA;
  }
-  double gain = detumbleParameter->gainD;
+  // convert uT to T
-  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
+  double magFieldT[3];
-  VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
+  VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
  // control law
  double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
  double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
  VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
  VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
  VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
  return returnvalue::OK;
 }
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@ -12,30 +12,22 @@
 class Detumble {
 public:
-  Detumble(AcsParameters *acsParameters_);
+  Detumble();
  virtual ~Detumble();
  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
  static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
  /* @brief: Load AcsParameters for this class
   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
   */
  void loadAcsParameters(AcsParameters *acsParameters_);
  ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
-                        const bool magFieldValid, double *magMom);
+                        const bool magFieldValid, double *magMom, double gain);
-  ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
+  ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
                            double dipolMax);
-  ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
+  ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
-
+                            const bool *magFieldValid, double *magMom, double gain);
  ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
                            const bool *magFieldValid, double *magMom);
 private:
  AcsParameters::DetumbleParameter *detumbleParameter;
  AcsParameters::MagnetorquesParameter *magnetorquesParameter;
 };
 #endif /*ACS_CONTROL_DETUMBLE_H_*/
--- a/mission/controller/acs/control/PtgCtrl.cpp
+++ b/mission/controller/acs/control/PtgCtrl.cpp
@ -1,10 +1,3 @@
 /*
 * PtgCtrl.cpp
 *
 *  Created on: 17 Jul 2022
 *      Author: Robin Marquardt
 */
 #include "PtgCtrl.h"
 #include <fsfw/globalfunctions/constants.h>
@ -16,16 +9,10 @@
 #include "../util/MathOperations.h"
-PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
+PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 PtgCtrl::~PtgCtrl() {}
 void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
  inertiaEIVE = &(acsParameters_->inertiaEIVE);
  rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
  rwMatrices = &(acsParameters_->rwMatrices);
 }
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
                     const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                     double *torqueRws) {
@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
  gainMatrixDiagonal[0][0] = gainVector[0];
  gainMatrixDiagonal[1][1] = gainVector[1];
  gainMatrixDiagonal[2][2] = gainVector[2];
-  MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
+  MatrixOperations<double>::multiply(
-                                     *gainMatrix, 3, 3, 3);
+      *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
  // Inverse of gainMatrix
  double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
  gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
  double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
+  MatrixOperations<double>::multiply(
-                                     3, 3);
+      *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
  MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
  //------------------------------------------------------------------------------------------------
@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
      pErrorSign[i] = pError[i];
    }
  }
-  //	Torque for quaternion error
+  // torque for quaternion error
  double torqueQuat[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
  VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
-  //  Torque for rate error
+  // torque for rate error
  double torqueRate[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
                                     torqueRate, 3, 3, 1);
  VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
  VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
-  //	Final commanded Torque for every reaction wheel
+  // final commanded Torque for every reaction wheel
  double torque[3] = {0, 0, 0};
  VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
  MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
@ -123,11 +111,13 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
  // calculating momentum of satellite and momentum of reaction wheels
  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
  double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
+  VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
-  MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
+                                      momentumRwU, 4);
-                                     1);
+  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(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
                                     momentumSat, 3, 3, 1);
  VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
  // calculating momentum error
  double deltaMomentum[3] = {0, 0, 0};
@ -147,53 +137,50 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
  double wheelMomentum[4] = {0, 0, 0, 0};
  double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
-  // Conversion to [rad/s] for further calculations
+  // conversion to [rad/s] for further calculations
  VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
  VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
  double diffRwSpeed[4] = {0, 0, 0, 0};
  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
-  VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
+  VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
                                      wheelMomentum, 4);
  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,
+  MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
                                                acsParameters->rwMatrices.nullspace,
                                                *nullSpaceMatrix, 4);
  MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
  VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
  VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
 }
-void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
+void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
  bool rwAvailable[4] = {
      (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
      (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
      (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
      (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
-  int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
+  int32_t currRwSpeed[4] = {
      sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
      sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
  for (uint8_t i = 0; i < 4; i++) {
    if (rwAvailable[i]) {
-      if (torqueMemory[i] != 0) {
+      if (rwCmdSpeeds[i] != 0) {
-        if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
+        if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
-          torqueMemory[i] = 0;
+            rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
-        } else {
+          if (currRwSpeed[i] == 0) {
-          torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
+            if (rwCmdSpeeds[i] > 0) {
              rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
            } else if (rwCmdSpeeds[i] < 0) {
              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;
          }
      } else {
        if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
            (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
          if (omegaRw[i] < omegaMemory[i]) {
            torqueMemory[i] = -1;
          } else {
            torqueMemory[i] = 1;
          }
          torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
        }
      }
    } else {
      torqueMemory[i] = 0;
    }
    omegaMemory[i] = omegaRw[i];
  }
 }
--- a/mission/controller/acs/control/PtgCtrl.h
+++ b/mission/controller/acs/control/PtgCtrl.h
@ -1,16 +1,3 @@
 /*
 * PtgCtrl.h
 *
 *  Created on: 17 Jul 2022
 *      Author: Robin Marquardt
 *
 *      @brief: This class handles the pointing control mechanism. Calculation of an commanded
 * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
 *
 *      @note: A description of the used algorithms can be found in
 *      https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
 */
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_
@ -23,6 +10,13 @@
 #include "eive/resultClassIds.h"
 class PtgCtrl {
  /*
   * @brief: This class handles the pointing control mechanism. Calculation of an commanded
   * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
   *
   * @note: A description of the used algorithms can be found in
   * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
   */
 public:
  /* @brief: Constructor
   * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
@ -33,13 +27,7 @@ class PtgCtrl {
  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
  static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
  /* @brief: Load AcsParameters for this class
   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
   */
  void loadAcsParameters(AcsParameters *acsParameters_);
  /* @brief: Calculates the needed torque for the pointing control mechanism
   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
   */
  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
@ -54,18 +42,12 @@ class PtgCtrl {
                    const int32_t *speedRw3, double *rwTrqNs);
  /* @brief: Commands the stiction torque in case wheel speed is to low
   * @param: sensorValues 	class containing all RW related values
   * 		 torqueCommand  modified torque after antistiction
   */
-  void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
+  void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
 private:
-  AcsParameters::RwHandlingParameters *rwHandlingParameters;
+  const AcsParameters *acsParameters;
  AcsParameters::InertiaEIVE *inertiaEIVE;
  AcsParameters::RwMatrices *rwMatrices;
  double torqueMemory[4] = {0, 0, 0, 0};
  double omegaMemory[4] = {0, 0, 0, 0};
 };
 #endif /* ACS_CONTROL_PTGCTRL_H_ */
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@ -1,10 +1,3 @@
 /*
 * SafeCtrl.cpp
 *
 *  Created on: 19 Apr 2022
 *      Author: Robin Marquardt
 */
 #include "SafeCtrl.h"
 #include <fsfw/globalfunctions/constants.h>
@ -15,19 +8,10 @@
 #include "../util/MathOperations.h"
-SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
+SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
  loadAcsParameters(acsParameters_);
  MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
                                           3);
 }
 SafeCtrl::~SafeCtrl() {}
 void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
  safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
  inertiaEIVE = &(acsParameters_->inertiaEIVE);
 }
 ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                 double *magFieldModel, bool magFieldModelValid,
                                 double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
@ -37,8 +21,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
    return SAFECTRL_MEKF_INPUT_INVALID;
  }
-  double kRate = safeModeControllerParameters->k_rate_mekf;
+  double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
-  double kAlign = safeModeControllerParameters->k_align_mekf;
+  double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
  //	Calc sunDirB ,magFieldB with mekf output and model
  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -71,6 +55,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
  //	Adding factor of inertia for axes
  MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
                                           *gainMatrixInertia, 3,
                                           3);  // why only for mekf one and not for no mekf
  MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
  // MagMom B (orthogonal torque)
@ -126,7 +113,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
  /* Only valid if angle between sun direction and magnetic field direction
   * is sufficiently large */
  double angleSunMag = acos(cosAngleSunMag);
-  if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
+  if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
    return returnvalue::FAILED;
  }
@ -135,8 +122,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
  VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
  // Torque Align calculation
-  double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
+  double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
-  double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
+  double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
  double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
  double crossSusSunRef[3] = {0, 0, 0};
@ -155,8 +142,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
  // Final torque
  double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
-  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
-                                     1);
+                                     torqueB, 3, 3, 1);
  // Magnetic moment
  double magMomB[3] = {0, 0, 0};
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/mission/controller/acs/control/SafeCtrl.h
@ -17,8 +17,6 @@ class SafeCtrl {
  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
  static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
  void loadAcsParameters(AcsParameters *acsParameters_);
  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
                         bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
                         double *satRateMekf, bool rateMekfValid, double *sunDirRef,
@ -32,8 +30,7 @@ class SafeCtrl {
 protected:
 private:
-  AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
+  AcsParameters *acsParameters;
  AcsParameters::InertiaEIVE *inertiaEIVE;
  double gainMatrixInertia[3][3];
  double magFieldBState[3];
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
  // GPS Processed
  GC_LATITUDE,
  GD_LONGITUDE,
  ALTITUDE,
  GPS_POSITION,
  GPS_VELOCITY,
  // MEKF
@ -109,7 +110,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
 static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
 static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
-static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
+static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t MEKF_SET_ENTRIES = 3;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
  lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
  lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
  lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
  lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
  lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);