Merge pull request 'Cleaner Calculation of MGM and SUS Rate' (#775) from mgm-sus-rate-calc into main

Reviewed-on: #775
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -28,6 +28,8 @@ will consitute of a breaking change warranting a new major release:
   
 ## Changed
 - GPS Fix has changed event is no longer triggered for the EM
+- MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available.
+  The stored value of the last timestep will now be reset, if no actual value is available.
 
 # [v6.3.0] 2023-08-03
 

mission/controller/acs/SensorProcessing.cpp

@@ -1,19 +1,5 @@
 #include "SensorProcessing.h"
 
-#include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/constants.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <fsfw/globalfunctions/timevalOperations.h>
-#include <math.h>
-
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "Igrf13Model.h"
-#include "util/MathOperations.h"
-
-using namespace Math;
-
 SensorProcessing::SensorProcessing() {}
 
 SensorProcessing::~SensorProcessing() {}
@@ -54,6 +40,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
         mgmDataProcessed->magIgrfModel.setValid(gpsValid);
       }
     }
+    std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot));
     return;
   }
   float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
@@ -141,14 +128,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool mgmVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
-  if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) {
-    for (uint8_t i = 0; i < 3; i++) {
-      mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
-      savedMgmVecTot[i] = mgmVecTot[i];
-      mgmVecTotDerivativeValid = true;
-    }
+  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);
+    mgmVecTotDerivativeValid = true;
   }
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
+  std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
 
   if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
       mgmDataProcessed->mgmVecTotDerivative.isValid()) {
@@ -277,6 +264,7 @@ void SensorProcessing::processSus(
         susDataProcessed->sunIjkModel.setValid(true);
       }
     }
+    std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
     return;
   }
 
@@ -365,13 +353,13 @@ void SensorProcessing::processSus(
   double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool susVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
-  if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) {
-    for (uint8_t i = 0; i < 3; i++) {
-      susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
-      savedSusVecTot[i] = susVecTot[i];
-      susVecTotDerivativeValid = true;
-    }
+  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);
+    susVecTotDerivativeValid = true;
   }
+  std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
   if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
       susDataProcessed->susVecTotDerivative.isValid()) {
     lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,

mission/controller/acs/SensorProcessing.h

@@ -1,15 +1,22 @@
 #ifndef SENSORPROCESSING_H_
 #define SENSORPROCESSING_H_
 
+#include <common/config/eive/resultClassIds.h>
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/constants.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/globalfunctions/timevalOperations.h>
 #include <fsfw/returnvalues/returnvalue.h>
-#include <stdint.h>  //uint8_t
-#include <time.h>    /*purpose, timeval ?*/
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/Igrf13Model.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/acs/SusConverter.h>
+#include <mission/controller/acs/util/MathOperations.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "AcsParameters.h"
-#include "SensorValues.h"
-#include "SusConverter.h"
-#include "eive/resultClassIds.h"
+#include <cmath>
 
 class SensorProcessing {
  public: