i hope i get a medal from Robin for this

mission/controller/AcsController.cpp

@@ -122,26 +122,6 @@ void AcsController::performControlOperation() {
     }
   }
 
-  timeval timeAbsolute;
-  Clock::getClock_timeval(&timeAbsolute);
-  timeval timeRelative;
-  Clock::getClockMonotonic(&timeRelative);
-
-  ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
-  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
-    triggerEvent(acs::TLE_TOO_OLD);
-    tleTooOldFlag = true;
-  } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
-    tleTooOldFlag = false;
-  }
-
-  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
-                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
-                                                        &gyrDataProcessed, &fusedRotRateData);
-  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &mekfData, &acsParameters);
-
   switch (internalState) {
     case InternalState::STARTUP: {
       initialCountdown.resetTimer();
@@ -156,25 +136,7 @@ void AcsController::performControlOperation() {
     }
     case InternalState::READY: {
       if (mode != MODE_OFF) {
-        switch (mode) {
+        performAttitudeControl();
-          case acs::SAFE:
-            switch (submode) {
-              case SUBMODE_NONE:
-                performSafe();
-                break;
-              case acs::DETUMBLE:
-                performDetumble();
-                break;
-            }
-            break;
-          case acs::PTG_IDLE:
-          case acs::PTG_TARGET:
-          case acs::PTG_TARGET_GS:
-          case acs::PTG_NADIR:
-          case acs::PTG_INERTIAL:
-            performPointingCtrl();
-            break;
-        }
       }
       break;
     }
@@ -183,23 +145,29 @@ void AcsController::performControlOperation() {
   }
 }
 
-void AcsController::performSafe() {
+void AcsController::performAttitudeControl() {
-  timeval now;
+  timeval timeAbsolute;
-  Clock::getClock_timeval(&now);
+  Clock::getClock_timeval(&timeAbsolute);
+  timeval timeRelative;
+  Clock::getClockMonotonic(&timeRelative);
 
-  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
   if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
     triggerEvent(acs::TLE_TOO_OLD);
     tleTooOldFlag = true;
   } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
     tleTooOldFlag = false;
   }
-  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+
-                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
+  sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed,
+                           &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
                                                         &gyrDataProcessed, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                               &susDataProcessed, &mekfData, &acsParameters);
+
+  switch (mode) {
+    case acs::SAFE:
       if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
           result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
         if (not mekfInvalidFlag) {
@@ -215,7 +183,55 @@ void AcsController::performSafe() {
         triggerEvent(acs::MEKF_RECOVERY);
         mekfInvalidFlag = false;
       }
+      switch (submode) {
+        case SUBMODE_NONE:
+          performSafe();
+          break;
+        case acs::DETUMBLE:
+          performDetumble();
+          break;
+      }
+      break;
+    case acs::PTG_IDLE:
+    case acs::PTG_TARGET:
+    case acs::PTG_TARGET_GS:
+    case acs::PTG_NADIR:
+    case acs::PTG_INERTIAL:
+      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+          result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+        mekfInvalidCounter++;
+        if (not mekfInvalidFlag) {
+          triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
+          mekfInvalidFlag = true;
+        }
+        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+          triggerEvent(acs::MEKF_AUTOMATIC_RESET);
+          navigation.resetMekf(&mekfData);
+          mekfLost = true;
+        }
+        if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
+          // Trigger this so STR FDIR can set the device faulty.
+          EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
+                                       0);
+          mekfInvalidCounter = 0;
+        }
+        commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
+                         cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+                         acsParameters.rwHandlingParameters.rampTime);
+        return;
+      } else {
+        if (mekfInvalidFlag) {
+          triggerEvent(acs::MEKF_RECOVERY);
+          mekfInvalidFlag = false;
+        }
+        mekfInvalidCounter = 0;
+      }
+      performPointingCtrl();
+      break;
+  }
+}
 
+void AcsController::performSafe() {
   // get desired satellite rate, sun direction to align to and inertia
   double sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir);
@@ -316,37 +332,6 @@ void AcsController::performSafe() {
 }
 
 void AcsController::performDetumble() {
-  timeval now;
-  Clock::getClock_timeval(&now);
-
-  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
-  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
-    triggerEvent(acs::TLE_TOO_OLD);
-    tleTooOldFlag = true;
-  } else {
-    tleTooOldFlag = false;
-  }
-  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
-                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
-                                                        &gyrDataProcessed, &fusedRotRateData);
-  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &mekfData, &acsParameters);
-  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
-      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
-    if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
-      mekfInvalidFlag = true;
-    }
-    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
-      triggerEvent(acs::MEKF_AUTOMATIC_RESET);
-      navigation.resetMekf(&mekfData);
-      mekfLost = true;
-    }
-  } else if (mekfInvalidFlag) {
-    triggerEvent(acs::MEKF_RECOVERY);
-    mekfInvalidFlag = false;
-  }
   acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
       mgmDataProcessed.mgmVecTotDerivative.isValid(),
@@ -409,51 +394,9 @@ void AcsController::performDetumble() {
 }
 
 void AcsController::performPointingCtrl() {
-  timeval now;
-  Clock::getClock_timeval(&now);
-
-  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
-  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
-    triggerEvent(acs::TLE_TOO_OLD);
-    tleTooOldFlag = true;
-  } else {
-    tleTooOldFlag = false;
-  }
-  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
-                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &mekfData, &acsParameters);
-  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
-      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
-    mekfInvalidCounter++;
-    if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
-      mekfInvalidFlag = true;
-    }
-    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
-      triggerEvent(acs::MEKF_AUTOMATIC_RESET);
-      navigation.resetMekf(&mekfData);
-      mekfLost = true;
-    }
-    if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
-      // Trigger this so STR FDIR can set the device faulty.
-      EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
-      mekfInvalidCounter = 0;
-    }
-    commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
-                     cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
-                     acsParameters.rwHandlingParameters.rampTime);
-    return;
-  } else {
-    if (mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_RECOVERY);
-      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);
+  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
     if (multipleRwUnavailableCounter >=
         acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {

mission/controller/AcsController.h

@@ -37,6 +37,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
                              uint16_t startAtIndex) override;
 
  protected:
+  void performAttitudeControl();
   void performSafe();
   void performDetumble();
   void performPointingCtrl();