changed mekf events again

mission/acsDefs.h

@@ -25,10 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
 static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
 //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
 static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
-//!< MEKF was not able to compute a solution during safe or detumble mode.
+//!< MEKF was not able to compute a solution.
-static constexpr Event MEKF_INVALID_SAFE_MODE = MAKE_EVENT(3, severity::INFO);
+static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
-//!< MEKF was not able to compute a solution during any pointing ACS mode.
+//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.
-static constexpr Event MEKF_INVALID_PTG_MODE = MAKE_EVENT(4, severity::HIGH);
+static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH);
 
 extern const char* getModeStr(AcsMode mode);
 

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),
@@ -129,10 +127,9 @@ void AcsController::performSafe() {
   if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
       result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
     if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
+      triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
-    return;
   } else {
     mekfInvalidFlag = false;
   }
@@ -157,10 +154,9 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
-  //	Detumble check and switch
+  // detumble check and switch
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
           acsParameters.detumbleParameter.omegaDetumbleStart) {
@@ -196,10 +192,9 @@ void AcsController::performDetumble() {
   if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
       result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
     if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
+      triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
-    return;
   } else {
     mekfInvalidFlag = false;
   }
@@ -207,7 +202,6 @@ void AcsController::performDetumble() {
   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() &&
@@ -245,21 +239,29 @@ void AcsController::performPointingCtrl() {
   if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
       result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
     if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_PTG_MODE);
+      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 rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    multipleRwUnavailableCounter++;
     if (multipleRwUnavailableCounter > 4) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
     }
+    multipleRwUnavailableCounter++;
     return;
   } else {
     multipleRwUnavailableCounter = 0;
@@ -387,12 +389,10 @@ 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);
 
   updateCtrlValData(targetQuat, errorQuat, errorAngle);

mission/controller/AcsController.h

@@ -50,12 +50,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   Detumble detumble;
   PtgCtrl ptgCtrl;
 
-  uint8_t detumbleCounter;
-  uint8_t multipleRwUnavailableCounter;
-
   ParameterHelper parameterHelper;
 
+  uint8_t detumbleCounter = 0;
+  uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = true;
+  uint8_t mekfInvalidCounter = 0;
+  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
+  int16_t cmdDipolMtqs[3] = {0, 0, 0};
 
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;