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::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::norm(gyrVecTot, 3) != 0 and gyrDataProcessed->gyrVecTot.isValid()) { lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight); }