diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 15e7767c..c1030c7b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -141,7 +141,7 @@ 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) { + 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]; @@ -394,7 +394,7 @@ 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) { + 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]; @@ -581,8 +581,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); - if (validSavedPosSatE && - (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + if (validSavedPosSatE and + (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and + (gpsUnixSeconds - timeOfSavedPosSatE) > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);