From e05d9d4b2a14d5c132eddddcbc243b6a9b528c1f Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 17 Apr 2023 09:29:50 +0200
Subject: [PATCH] prevent low pass filter if no sensor data is available

---
 mission/controller/acs/SensorProcessing.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index c1030c7b..4cc15a16 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -150,7 +150,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   }
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
 
-  if (mgmDataProcessed->mgmVecTotDerivative.isValid()) {
+  if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
+      mgmDataProcessed->mgmVecTotDerivative.isValid()) {
     lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value,
                   mgmParameters->mgmDerivativeFilterWeight);
   }
@@ -533,7 +534,7 @@ void SensorProcessing::processGyr(
     }
   }
 
-  if (gyrDataProcessed->gyrVecTot.isValid()) {
+  if (VectorOperations<double>::norm(gyrVecTot, 3) != 0 and gyrDataProcessed->gyrVecTot.isValid()) {
     lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight);
   }