From 110c822f41119266e035f11bc9b56ec4ffa572af Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 14 Aug 2023 10:08:51 +0200
Subject: [PATCH] cleaner calculation of sus rate

---
 mission/controller/acs/SensorProcessing.cpp | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index d5ec5f7d..6dad0a86 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -142,8 +142,6 @@ 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);
-  sif::debug << "timeDiff = " << timeDiff << std::endl;
-  sif::debug << "tv_sec = " << (double)timeOfSavedMagFieldEst.tv_sec << std::endl;
   if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
       VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
     VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
@@ -280,6 +278,7 @@ void SensorProcessing::processSus(
         susDataProcessed->sunIjkModel.setValid(true);
       }
     }
+    std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
     return;
   }
 
@@ -368,13 +367,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,