diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 511cae35..d5ec5f7d 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,11 +6,11 @@ #include #include #include -#include +#include +#include +#include -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "Igrf13Model.h" -#include "util/MathOperations.h" +#include using namespace Math; @@ -54,6 +54,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 +142,16 @@ 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; - } + 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::norm(savedMgmVecTot, 3) != 0) { + VectorOperations::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); + VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); + mgmVecTotDerivativeValid = true; } timeOfSavedMagFieldEst = timeOfMgmMeasurement; + std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and mgmDataProcessed->mgmVecTotDerivative.isValid()) {