CHANGELOG.md

@@ -24,6 +24,8 @@ will consitute of a breaking change warranting a new major release:
   A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even
   though it is there.
 - Added action cmd to read the currently stored TLE.
+- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
+  the time difference.
 
 # [v7.4.0] 2023-11-30
 

mission/controller/AcsController.cpp

@@ -141,8 +141,56 @@ void AcsController::performControlOperation() {
     }
     case InternalState::READY: {
       if (mode != MODE_OFF) {
+        performAttitudeControl();
+      }
+      break;
+    }
+    default:
+      break;
+  }
+}
+
+void AcsController::performAttitudeControl() {
+  Clock::getClock_timeval(&timeAbsolute);
+  Clock::getClockMonotonic(&timeRelative);
+
+  if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) {
+    timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative);
+  }
+  oldTimeRelative = 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(timeAbsolute, timeDelta, &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 and
+          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 and not mekfLost) {
+          triggerEvent(acs::MEKF_AUTOMATIC_RESET);
+          navigation.resetMekf(&mekfData);
+          mekfLost = true;
+        }
+      } else if (mekfInvalidFlag) {
+        triggerEvent(acs::MEKF_RECOVERY);
+        mekfInvalidFlag = false;
+      }
       switch (submode) {
         case SUBMODE_NONE:
           performSafe();
@@ -157,50 +205,41 @@ void AcsController::performControlOperation() {
     case acs::PTG_TARGET_GS:
     case acs::PTG_NADIR:
     case acs::PTG_INERTIAL:
+      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
+          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 and not 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;
   }
 }
-      break;
-    }
-    default:
-      break;
-  }
-}
 
 void AcsController::performSafe() {
-  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 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);
-  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;
-  }
-
   // get desired satellite rate, sun direction to align to and inertia
   double sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir);
@@ -301,37 +340,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(),
@@ -394,51 +402,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) {
@@ -460,7 +426,7 @@ void AcsController::performPointingCtrl() {
 
   switch (mode) {
     case acs::PTG_IDLE:
-      guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
+      guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
                                 targetSatRotRate);
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                           targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@@ -481,7 +447,7 @@ void AcsController::performPointingCtrl() {
       break;
 
     case acs::PTG_TARGET:
-      guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+      guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
                                       gpsDataProcessed.gpsVelocity.value, targetQuat,
                                       targetSatRotRate);
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
@@ -505,7 +471,7 @@ void AcsController::performPointingCtrl() {
       break;
 
     case acs::PTG_TARGET_GS:
-      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
+      guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
                                susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                           targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
@@ -526,9 +492,9 @@ void AcsController::performPointingCtrl() {
       break;
 
     case acs::PTG_NADIR:
-      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+      guidance.targetQuatPtgNadirThreeAxes(
-                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
+          timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
-                                           targetSatRotRate);
+          gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
       guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                           targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
                           acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,

mission/controller/AcsController.h

@@ -45,6 +45,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
                              uint16_t startAtIndex) override;
 
  protected:
+  void performAttitudeControl();
   void performSafe();
   void performDetumble();
   void performPointingCtrl();
@@ -59,6 +60,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   SdCardMountedIF& sdcMan;
 
+  timeval timeAbsolute;
+  timeval timeRelative;
+  double timeDelta = 0.0;
+  timeval oldTimeRelative;
+
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
   FusedRotationEstimation fusedRotationEstimation;

mission/controller/PowerController.cpp

@@ -157,7 +157,12 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
 
 void PowerController::calculateStateOfCharge() {
   // get time
-  Clock::getClock_timeval(&now);
+  Clock::getClockMonotonic(&now);
+  double timeDelta = 0.0;
+  if (now.tv_sec != 0 and oldTime.tv_sec != 0) {
+    timeDelta = timevalOperations::toDouble(now - oldTime);
+  }
+  oldTime = now;
 
   // update EPS HK values
   ReturnValue_t result = updateEpsData();
@@ -173,8 +178,6 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.setValidity(false, true);
       }
     }
-    // store time for next run
-    oldTime = now;
     return;
   }
 
@@ -195,12 +198,10 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
       }
     }
-    // store time for next run
-    oldTime = now;
     return;
   }
 
-  result = calculateCoulombCounterCharge();
+  result = calculateCoulombCounterCharge(timeDelta);
   if (result != returnvalue::OK) {
     // notifying events have already been triggered
     {
@@ -215,9 +216,6 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
       }
     }
-    // store time for next run
-    oldTime = now;
-    return;
   }
 
   // commit to dataset
@@ -231,8 +229,6 @@ void PowerController::calculateStateOfCharge() {
       pwrCtrlCoreHk.setValidity(true, true);
     }
   }
-  // store time for next run
-  oldTime = now;
 }
 
 void PowerController::watchStateOfCharge() {
@@ -285,12 +281,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
   return returnvalue::OK;
 }
 
-ReturnValue_t PowerController::calculateCoulombCounterCharge() {
+ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
-  double timeDiff = timevalOperations::toDouble(now - oldTime);
+  if (timeDelta == 0.0) {
-  if (timeDiff > maxAllowedTimeDiff) {
+    return returnvalue::FAILED;
+  }
+  if (timeDelta > maxAllowedTimeDiff) {
     // should not be a permanent state so no spam protection required
-    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
+    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
-    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
+    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
                << std::endl;
     return returnvalue::FAILED;
   }
@@ -298,7 +296,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge() {
     coulombCounterCharge = openCircuitVoltageCharge;
   } else {
     coulombCounterCharge =
-        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
+        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
     if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
       coulombCounterCharge = coulombCounterChargeUpperThreshold;
     }

mission/controller/PowerController.h

@@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   void calculateStateOfCharge();
   void watchStateOfCharge();
   ReturnValue_t calculateOpenCircuitVoltageCharge();
-  ReturnValue_t calculateCoulombCounterCharge();
+  ReturnValue_t calculateCoulombCounterCharge(double timeDelta);
   ReturnValue_t updateEpsData();
   float charge2stateOfCharge(float capacity, bool coulombCounter);
   ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);

mission/controller/acs/AcsParameters.h

@@ -75,9 +75,9 @@ class AcsParameters : public HasParametersIF {
                                        {-0.007534, 1.253879, 0.006812},
                                        {-0.037072, 0.006812, 1.313158}};
 
-    float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
+    double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
-    float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
+    double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
-    float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
+    double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
     float mgmVectorFilterWeight = 0.85;
     float mgmDerivativeFilterWeight = 0.99;
     uint8_t useMgm4 = false;
@@ -787,10 +787,10 @@ class AcsParameters : public HasParametersIF {
 
     /* 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(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
+    double gyr02variance[3] = {pow(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(4.6e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(6.1e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
-    float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
+    double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
     uint8_t preferAdis = false;
     float gyrFilterWeight = 0.6;
   } gyrHandlingParameters;

mission/controller/acs/Guidance.cpp

@@ -4,21 +4,21 @@
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>
+#include <mission/controller/acs/util/MathOperations.h>
 
+#include <cmath>
 #include <filesystem>
-
+#include <string>
-#include "string.h"
-#include "util/CholeskyDecomposition.h"
-#include "util/MathOperations.h"
 
 Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 Guidance::~Guidance() {}
 
-void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
+[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3],
-                                       double sunDirI[3], double refDirB[3], double quatBI[4],
+                                                      double velSatE[3], double sunDirI[3],
-                                       double targetQuat[4], double targetSatRotRate[3]) {
+                                                      double refDirB[3], double quatBI[4],
+                                                      double targetQuat[4],
+                                                      double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
@@ -38,7 +38,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
   MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -136,8 +136,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
   QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
 }
 
-void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
-                                      double targetQuat[4], double targetSatRotRate[3]) {
+                                      double posSatE[3], double velSatE[3], double targetQuat[4],
+                                      double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for target pointing
   //-------------------------------------------------------------------------------------
@@ -154,7 +155,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
   MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -199,11 +200,12 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
   QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
   int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+  targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
+void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
-                               double targetQuat[4], double targetSatRotRate[3]) {
+                               double posSatE[3], double sunDirI[3], double targetQuat[4],
+                               double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for ground station pointing
   //-------------------------------------------------------------------------------------
@@ -221,7 +223,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
   MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -263,10 +265,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
   QuaternionOperations::fromDcm(dcmTgt, targetQuat);
 
   int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+  targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
                                 double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to sun
@@ -298,10 +300,11 @@ void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQua
   //	Calculation of reference rotation rate
   //----------------------------------------------------------------------------
   int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+  targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
+                                                           double posSatE[3], double quatBI[4],
                                                            double targetQuat[4], double refDirB[3],
                                                            double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
@@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
   MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -362,7 +365,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
   QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
 }
 
-void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
+                                           double posSatE[3], double velSatE[3],
                                            double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
@@ -371,7 +375,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
   MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -407,7 +411,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
   QuaternionOperations::fromDcm(dcmTgt, targetQuat);
 
   int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
+  targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate);
 }
 
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
@@ -448,23 +452,21 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
 }
 
-void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
-                                  double *refSatRate) {
+                                  double quatInertialTarget[4], double *refSatRate) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target rotation rate
   //-------------------------------------------------------------------------------------
-  double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
-                       (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
   if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
     std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
   }
-  if (timeElapsed < timeElapsedMax) {
+  if (timeDelta < timeElapsedMax and timeDelta != 0.0) {
     double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
     QuaternionOperations::inverse(quatInertialTarget, q);
     QuaternionOperations::inverse(savedQuaternion, qS);
     double qDiff[4] = {0, 0, 0, 0};
     VectorOperations<double>::subtract(q, qS, qDiff, 4);
-    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
+    VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
 
     double tgtQuatVec[3] = {q[0], q[1], q[2]};
     double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
@@ -488,11 +490,7 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
     refSatRate[2] = 0;
   }
 
-  timeSavedQuaternion = now;
+  std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
-  savedQuaternion[0] = quatInertialTarget[0];
-  savedQuaternion[1] = quatInertialTarget[1];
-  savedQuaternion[2] = quatInertialTarget[2];
-  savedQuaternion[3] = quatInertialTarget[3];
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,

mission/controller/acs/Guidance.h

@@ -17,25 +17,26 @@ class Guidance {
 
   // Function to get the target quaternion and reference rotation rate from gps position and
   // position of the ground station
-  void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
+  void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
-                               double refDirB[3], double quatBI[4], double targetQuat[4],
+                               double sunDirI[3], double refDirB[3], double quatBI[4],
-                               double targetSatRotRate[3]);
+                               double targetQuat[4], double targetSatRotRate[3]);
-  void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
+  void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
-                              double targetSatRotRate[3]);
+                              double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
+  void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
-                       double targetSatRotRate[3]);
+                       double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
 
   // Function to get the target quaternion and reference rotation rate for sun pointing after ground
   // station
-  void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+  void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
                         double targetSatRotRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
-  void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+  void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
                                     double targetQuat[4], double refDirB[3], double refSatRate[3]);
-  void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+  void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
-                                   double targetQuat[4], double refSatRate[3]);
+                                   double posSatE[3], double velSatE[3], double targetQuat[4],
+                                   double refSatRate[3]);
 
   // @note: Calculates the error quaternion between the current orientation and the target
   // quaternion, considering a reference quaternion. Additionally the difference between the actual
@@ -48,8 +49,8 @@ class Guidance {
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                   double &errorAngle);
 
-  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+  void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
-                          double *targetSatRotRate);
+                          double quatInertialTarget[4], double *targetSatRotRate);
 
   // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
   // reation wheel 	maybe can be done in "commanding.h"
@@ -59,7 +60,6 @@ class Guidance {
   const AcsParameters *acsParameters;
 
   bool strBlindAvoidFlag = false;
-  timeval timeSavedQuaternion;
   double savedQuaternion[4] = {0, 0, 0, 0};
   double omegaRefSaved[3] = {0, 0, 0};
 

mission/controller/acs/SensorProcessing.cpp

@@ -1,5 +1,7 @@
 #include "SensorProcessing.h"
 
+#include <fsfw/timemanager/Stopwatch.h>
+
 SensorProcessing::SensorProcessing() {}
 
 SensorProcessing::~SensorProcessing() {}
@@ -7,7 +9,7 @@ SensorProcessing::~SensorProcessing() {}
 void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
                                   bool mgm1valid, const float *mgm2Value, bool mgm2valid,
                                   const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
-                                  bool mgm4valid, timeval timeOfMgmMeasurement,
+                                  bool mgm4valid, timeval timeAbsolute, double timeDelta,
                                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed) {
@@ -16,13 +18,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   double magIgrfModel[3] = {0.0, 0.0, 0.0};
   bool gpsValid = false;
   if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
+    // There seems to be a bug here, which causes the model vector to drift until infinity, if the
+    // model class is not initialized new every time. Works for now, but should be investigated.
     Igrf13Model igrf13;
     igrf13.schmidtNormalization();
-    igrf13.updateCoeffGH(timeOfMgmMeasurement);
+    igrf13.updateCoeffGH(timeAbsolute);
-    // maybe put a condition here, to only update after a full day, this
-    // class function has around 700 steps to perform
     igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
-                        gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
+                        gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
     gpsValid = true;
   }
   if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
@@ -129,14 +131,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   //-----------------------Mgm Rate Computation ---------------------------------------------------
   double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool mgmVecTotDerivativeValid = false;
-  double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
+  if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
-  if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
-      VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
     VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
-    VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
+    VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative,
+                                        3);
     mgmVecTotDerivativeValid = true;
   }
-  timeOfSavedMagFieldEst = timeOfMgmMeasurement;
   std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
 
   if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
@@ -177,11 +177,12 @@ void SensorProcessing::processSus(
     const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
     const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
     const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
-    timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
+    timeval timeAbsolute, double timeDelta,
+    const AcsParameters::SusHandlingParameters *susParameters,
     const AcsParameters::SunModelParameters *sunModelParameters,
     acsctrl::SusDataProcessed *susDataProcessed) {
   /* -------- Sun Model Direction (IJK frame) ------- */
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
 
   // Julean Centuries
   double sunIjkModel[3] = {0.0, 0.0, 0.0};
@@ -354,11 +355,10 @@ void SensorProcessing::processSus(
 
   double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool susVecTotDerivativeValid = false;
-  double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
+  if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
-  if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
-      VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
     VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
-    VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
+    VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative,
+                                        3);
     susVecTotDerivativeValid = true;
   }
   std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
@@ -367,7 +367,6 @@ void SensorProcessing::processSus(
     lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
                   susParameters->susRateFilterWeight);
   }
-  timeOfSavedSusDirEst = timeOfSusMeasurement;
   {
     PoolReadGuard pg(susDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -414,7 +413,7 @@ void SensorProcessing::processGyr(
     const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
     const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
     const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
-    timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
+    const AcsParameters::GyrHandlingParameters *gyrParameters,
     acsctrl::GyrDataProcessed *gyrDataProcessed) {
   bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
   bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
@@ -521,7 +520,7 @@ void SensorProcessing::processGyr(
 }
 
 void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
-                                  const double gpsAltitude, const double gpsUnixSeconds,
+                                  const double gpsAltitude, const double timeDelta,
                                   const bool validGps,
                                   const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
@@ -563,18 +562,14 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
     // Calculation of the satellite velocity in earth fixed frame
     double deltaDistance[3] = {0, 0, 0};
     MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
-    if (validSavedPosSatE and
+    if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
-        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
-        (gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
       VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
-      double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
+      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
-      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
     }
     savedPosSatE[0] = posSatE[0];
     savedPosSatE[1] = posSatE[1];
     savedPosSatE[2] = posSatE[2];
 
-    timeOfSavedPosSatE = gpsUnixSeconds;
     validSavedPosSatE = true;
 
     gpsSource = acs::GpsSource::GPS;
@@ -594,13 +589,15 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
   }
 }
 
-void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
+void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
+                               ACS::SensorValues *sensorValues,
                                acsctrl::MgmDataProcessed *mgmDataProcessed,
                                acsctrl::SusDataProcessed *susDataProcessed,
                                acsctrl::GyrDataProcessed *gyrDataProcessed,
                                acsctrl::GpsDataProcessed *gpsDataProcessed,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
+
   processGps(
       sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
       sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
@@ -617,7 +614,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
              sensorValues->mgm3Rm3100Set.fieldStrengths.value,
              sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
              sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
-             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
+             timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
+             mgmDataProcessed);
 
   processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
              sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@@ -631,8 +629,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
              sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
              sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
              sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
-             now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
+             timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
-             susDataProcessed);
+             &acsParameters->sunModelParameters, susDataProcessed);
 
   processGyr(
       sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@@ -646,7 +644,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
       sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
       sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
       sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
-      sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
+      sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(),
       &acsParameters->gyrHandlingParameters, gyrDataProcessed);
 }
 

mission/controller/acs/SensorProcessing.h

@@ -24,22 +24,21 @@ class SensorProcessing {
   SensorProcessing();
   virtual ~SensorProcessing();
 
-  void process(timeval now, ACS::SensorValues *sensorValues,
+  void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
                acsctrl::MgmDataProcessed *mgmDataProcessed,
                acsctrl::SusDataProcessed *susDataProcessed,
                acsctrl::GyrDataProcessed *gyrDataProcessed,
-               acsctrl::GpsDataProcessed *gpsDataProcessed,
+               acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
-               const AcsParameters *acsParameters);  // Will call protected functions
+
  private:
   static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
   static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
   static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
 
  protected:
-  // short description needed for every function
   void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
                   const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
-                  const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
+                  const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                   acsctrl::MgmDataProcessed *mgmDataProcessed);
@@ -52,7 +51,7 @@ class SensorProcessing {
                   bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
                   const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
                   bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
-                  timeval timeOfSusMeasurement,
+                  timeval timeAbsolute, double timeDelta,
                   const AcsParameters::SusHandlingParameters *susParameters,
                   const AcsParameters::SunModelParameters *sunModelParameters,
                   acsctrl::SusDataProcessed *susDataProcessed);
@@ -65,7 +64,6 @@ class SensorProcessing {
                   bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
                   const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
                   bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
-                  timeval timeOfGyrMeasurement,
                   const AcsParameters::GyrHandlingParameters *gyrParameters,
                   acsctrl::GyrDataProcessed *gyrDataProcessed);
 
@@ -77,13 +75,9 @@ class SensorProcessing {
   void lowPassFilter(double *newValue, double *oldValue, const double weight);
 
   double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
-  timeval timeOfSavedMagFieldEst;
   double savedSusVecTot[3] = {0.0, 0.0, 0.0};
-  timeval timeOfSavedSusDirEst;
-  bool validMagField = false;
 
   double savedPosSatE[3] = {0.0, 0.0, 0.0};
-  double timeOfSavedPosSatE = 0.0;
   bool validSavedPosSatE = false;
 
   SusConverter susConverter;