this might just work

mission/controller/AcsController.cpp

@@ -146,11 +146,14 @@ void AcsController::performControlOperation() {
 }
 
 void AcsController::performAttitudeControl() {
-  timeval timeAbsolute;
   Clock::getClock_timeval(&timeAbsolute);
-  timeval timeRelative;
   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);
@@ -159,7 +162,7 @@ void AcsController::performAttitudeControl() {
     tleTooOldFlag = false;
   }
 
-  sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed,
+  sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
                            &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
                                                         &gyrDataProcessed, &fusedRotRateData);
@@ -168,13 +171,13 @@ void AcsController::performAttitudeControl() {
 
   switch (mode) {
     case acs::SAFE:
-      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      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 && !mekfLost) {
+        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
           triggerEvent(acs::MEKF_AUTOMATIC_RESET);
           navigation.resetMekf(&mekfData);
           mekfLost = true;
@@ -197,14 +200,14 @@ void AcsController::performAttitudeControl() {
     case acs::PTG_TARGET_GS:
     case acs::PTG_NADIR:
     case acs::PTG_INERTIAL:
-      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      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 && !mekfLost) {
+        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
           triggerEvent(acs::MEKF_AUTOMATIC_RESET);
           navigation.resetMekf(&mekfData);
           mekfLost = true;
@@ -418,7 +421,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);
@@ -439,7 +442,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,
@@ -463,7 +466,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);
@@ -484,9 +487,9 @@ void AcsController::performPointingCtrl() {
       break;
 
     case acs::PTG_NADIR:
-      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
-                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
-                                           targetSatRotRate);
+      guidance.targetQuatPtgNadirThreeAxes(
+          timeAbsolute, timeDelta, 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,

mission/controller/AcsController.h

@@ -50,6 +50,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   bool enableHkSets = false;
 
+  timeval timeAbsolute;
+  timeval timeRelative;
+  double timeDelta = 0.0;
+  timeval oldTimeRelative;
+
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
   FusedRotationEstimation fusedRotationEstimation;

mission/controller/acs/Guidance.cpp

@@ -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],
-                                      double targetQuat[4], double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
+                                      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],
-                               double targetQuat[4], double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
+                               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,7 +300,7 @@ 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],
@@ -362,7 +364,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 +374,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 +410,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 +451,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],
-                                  double *refSatRate) {
+void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
+                                  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) {
     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 +489,7 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
     refSatRate[2] = 0;
   }
 
-  timeSavedQuaternion = now;
-  savedQuaternion[0] = quatInertialTarget[0];
-  savedQuaternion[1] = quatInertialTarget[1];
-  savedQuaternion[2] = quatInertialTarget[2];
-  savedQuaternion[3] = quatInertialTarget[3];
+  std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
 }
 
 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],
-                               double refDirB[3], double quatBI[4], double targetQuat[4],
-                               double targetSatRotRate[3]);
-  void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
-                              double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
-                       double targetSatRotRate[3]);
+  void targetQuatPtgSingleAxis(const double timeDelta, double posSatE[3], double velSatE[3],
+                               double sunDirI[3], double refDirB[3], double quatBI[4],
+                               double targetQuat[4], double targetSatRotRate[3]);
+  void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
+                              double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
+  void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[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 double timeDelta, 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],
-                                   double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
+                                   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],
-                          double *targetSatRotRate);
+  void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
+                          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

@@ -588,7 +588,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
   }
 }
 
-void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative,
+void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
                                ACS::SensorValues *sensorValues,
                                acsctrl::MgmDataProcessed *mgmDataProcessed,
                                acsctrl::SusDataProcessed *susDataProcessed,
@@ -596,11 +596,6 @@ void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative,
                                acsctrl::GpsDataProcessed *gpsDataProcessed,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
-  double timeDelta = 0;
-  if (timeRelative.tv_sec != 0 and savedTimeRelative.tv_sec != 0) {
-    timeDelta = timevalOperations::toDouble(timeRelative - savedTimeRelative);
-  }
-  savedTimeRelative = timeRelative;
 
   processGps(
       sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,

mission/controller/acs/SensorProcessing.h

@@ -24,7 +24,7 @@ class SensorProcessing {
   SensorProcessing();
   virtual ~SensorProcessing();
 
-  void process(timeval timeAbsolute, timeval timeRelative, ACS::SensorValues *sensorValues,
+  void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
                acsctrl::MgmDataProcessed *mgmDataProcessed,
                acsctrl::SusDataProcessed *susDataProcessed,
                acsctrl::GyrDataProcessed *gyrDataProcessed,
@@ -74,7 +74,6 @@ class SensorProcessing {
 
   void lowPassFilter(double *newValue, double *oldValue, const double weight);
 
-  timeval savedTimeRelative;
   double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
   double savedSusVecTot[3] = {0.0, 0.0, 0.0};