Merge branch 'develop' into eggert/acs-ctrl-action-cmds

mission/controller/AcsController.cpp

@@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId)
       safeCtrl(&acsParameters),
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
-      detumbleCounter{0},
-      multipleRwUnavailableCounter{0},
       parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
@@ -28,6 +26,14 @@ AcsController::AcsController(object_id_t objectId)
       ctrlValData(this),
       actuatorCmdData(this) {}
 
+ReturnValue_t AcsController::initialize() {
+  ReturnValue_t result = parameterHelper.initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return ExtendedControllerBase::initialize();
+}
+
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   ReturnValue_t result = actionHelper.handleActionMessage(message);
   if (result == returnvalue::OK) {
@@ -133,17 +139,24 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
-  // give desired satellite rate and sun direction to align
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+  } else {
+    mekfInvalidFlag = false;
+  }
+  // get desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
   bool magMomMtqValid = false;
-  if (validMekf == returnvalue::OK) {
+  if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
     safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
                       mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
                       susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
@@ -158,24 +171,9 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
-  {
-    PoolReadGuard pg(&ctrlValData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      double unitQuat[4] = {0, 0, 0, 1};
-      std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
-      ctrlValData.tgtQuat.setValid(false);
-      std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
-      ctrlValData.errQuat.setValid(false);
-      ctrlValData.errAng.value = errAng;
-      ctrlValData.errAng.setValid(true);
-      ctrlValData.setValidity(true, false);
-    }
-  }
-
-  //	Detumble check and switch
+  // detumble check and switch
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
           acsParameters.detumbleParameter.omegaDetumbleStart) {
@@ -193,20 +191,8 @@ void AcsController::performSafe() {
     triggerEvent(acs::SAFE_RATE_VIOLATION);
   }
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      int32_t zeroVec[4] = {0, 0, 0, 0};
-      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetTorque.setValid(false);
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.mtqTargetDipole.setValid(true);
-      actuatorCmdData.setValidity(true, false);
-    }
-  }
-
+  updateCtrlValData(errAng);
+  updateActuatorCmdData(cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
   //                   acsParameters.rwHandlingParameters.rampTime);
@@ -218,15 +204,21 @@ void AcsController::performDetumble() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+  } else {
+    mekfInvalidFlag = false;
+  }
   double magMomMtq[3] = {0, 0, 0};
   detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                    mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
                    mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
   if (mekfData.satRotRateMekf.isValid() &&
@@ -246,19 +238,8 @@ void AcsController::performDetumble() {
     triggerEvent(acs::SAFE_RATE_RECOVERY);
   }
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
-      actuatorCmdData.rwTargetTorque.setValid(false);
-      std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.mtqTargetDipole.setValid(true);
-      actuatorCmdData.setValidity(true, false);
-    }
-  }
-
+  disableCtrlValData();
+  updateActuatorCmdData(cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
   //                   acsParameters.rwHandlingParameters.rampTime);
@@ -270,22 +251,34 @@ void AcsController::performPointingCtrl() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
-  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
-  double quatRef[4] = {0, 0, 0, 0};
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+    if (mekfInvalidCounter > 4) {
+      triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
+    }
+    mekfInvalidCounter++;
+    commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
+                     cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+                     acsParameters.rwHandlingParameters.rampTime);
+    return;
+  } else {
+    mekfInvalidFlag = false;
+    mekfInvalidCounter = 0;
+  }
   uint8_t enableAntiStiction = true;
-
-  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    multipleRwUnavailableCounter++;
     if (multipleRwUnavailableCounter > 4) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
     }
+    multipleRwUnavailableCounter++;
     return;
   } else {
     multipleRwUnavailableCounter = 0;
@@ -294,40 +287,37 @@ void AcsController::performPointingCtrl() {
   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};
   switch (submode) {
     case acs::PTG_IDLE:
-      guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
-                          targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
-                     *rwPseudoInv, torquePtgRws);
+      guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgNullspace(
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &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);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          &acsParameters.idleModeControllerParameters, 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.idleModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET:
-      guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
-                                      refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+                                      gpsDataProcessed.gpsVelocity.value, targetQuat,
+                                      targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
+                          acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -340,17 +330,15 @@ void AcsController::performPointingCtrl() {
           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;
       break;
 
     case acs::PTG_TARGET_GS:
-      guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
-                               targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
+                               susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -363,16 +351,18 @@ void AcsController::performPointingCtrl() {
           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;
       break;
 
     case acs::PTG_NADIR:
-      guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
-                                     refSatRate);
-      std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
-      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
+                                           targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
+                          acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -385,16 +375,17 @@ void AcsController::performPointingCtrl() {
           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.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
-      guidance.inertialQuatPtg(targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
+      std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                   4 * sizeof(double));
-      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
+                          acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -407,6 +398,7 @@ void AcsController::performPointingCtrl() {
           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.inertialModeControllerParameters.enableAntiStiction;
       break;
   }
 
@@ -414,24 +406,14 @@ void AcsController::performPointingCtrl() {
     ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
   }
 
-  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
                             sensorValues.rw2Set.currSpeed.value,
                             sensorValues.rw3Set.currSpeed.value,
                             sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.setValidity(true, true);
-    }
-  }
-
+  updateCtrlValData(targetQuat, errorQuat, errorAngle);
+  updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
   //                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@@ -467,6 +449,67 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
   return returnvalue::OK;
 }
 
+void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
+  double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
+  int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
+  updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
+}
+
+void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
+                                          int16_t mtqTargetDipole[3]) {
+  {
+    PoolReadGuard pg(&actuatorCmdData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
+      actuatorCmdData.setValidity(true, true);
+    }
+  }
+}
+
+void AcsController::updateCtrlValData(double errAng) {
+  double unitQuat[4] = {0, 0, 0, 1};
+  {
+    PoolReadGuard pg(&ctrlValData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
+      ctrlValData.tgtQuat.setValid(false);
+      std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
+      ctrlValData.errQuat.setValid(false);
+      ctrlValData.errAng.value = errAng;
+      ctrlValData.errAng.setValid(true);
+      ctrlValData.setValidity(true, false);
+    }
+  }
+}
+
+void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
+  {
+    PoolReadGuard pg(&ctrlValData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
+      std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
+      ctrlValData.errAng.value = errAng;
+      ctrlValData.setValidity(true, true);
+    }
+  }
+}
+
+void AcsController::disableCtrlValData() {
+  double unitQuat[4] = {0, 0, 0, 1};
+  double errAng = 0;
+  {
+    PoolReadGuard pg(&ctrlValData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
+      std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
+      ctrlValData.errAng.value = errAng;
+      ctrlValData.setValidity(false, true);
+    }
+  }
+}
+
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
   // MGM Raw
@@ -540,11 +583,13 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
+  localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
   poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
+  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
   poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
   // Actuator CMD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
@@ -811,11 +856,3 @@ void AcsController::copyGyrData() {
     }
   }
 }
-
-ReturnValue_t AcsController::initialize() {
-  ReturnValue_t result = parameterHelper.initialize();
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  return ExtendedControllerBase::initialize();
-}