From 5a7f0d08a42af06a1fdae8059999d56506a90d2f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 09:53:33 +0200 Subject: [PATCH 1/5] cleaner calculation of mgm rate --- mission/controller/acs/SensorProcessing.cpp | 23 ++++++++++++--------- 1 file changed, 13 insertions(+), 10 deletions(-) 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()) { From 110c822f41119266e035f11bc9b56ec4ffa572af Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 10:08:51 +0200 Subject: [PATCH 2/5] 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::norm(savedMgmVecTot, 3) != 0) { VectorOperations::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::norm(savedSusVecTot, 3) != 0) { + VectorOperations::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); + VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); + susVecTotDerivativeValid = true; } + std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); if (VectorOperations::norm(susVecTotDerivative, 3) != 0 and susDataProcessed->susVecTotDerivative.isValid()) { lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, From a99895a2b65ab1b77cdbd8333121f6a2e19d1ae8 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 10:15:33 +0200 Subject: [PATCH 3/5] cleanup --- mission/controller/acs/SensorProcessing.cpp | 14 -------------- mission/controller/acs/SensorProcessing.h | 21 ++++++++++++++------- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 6dad0a86..27067dca 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,19 +1,5 @@ #include "SensorProcessing.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace Math; - SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 6dbc5d58..6a167d83 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,15 +1,22 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include +#include +#include +#include +#include +#include +#include #include -#include //uint8_t -#include /*purpose, timeval ?*/ +#include +#include +#include +#include +#include +#include -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "SensorValues.h" -#include "SusConverter.h" -#include "eive/resultClassIds.h" +#include class SensorProcessing { public: From 9ba8c02e915c1b2c9a064d713ce26b0137052969 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 10:20:28 +0200 Subject: [PATCH 4/5] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f1f275c..23ab0457 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,10 @@ will consitute of a breaking change warranting a new major release: - The handling function of the GPS data is only called once per GPS read. This should remove the fake fix-has-changed events. +## Changed +- MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available. + The stored value of the last timestep will now be reset, if no actual value is available. + # [v6.3.0] 2023-08-03 ## Fixed From dd5b858666091f8150107dc5e442dded47a5ac21 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 14 Aug 2023 14:59:56 +0200 Subject: [PATCH 5/5] changelog --- CHANGELOG.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 31c50b99..ffd14c0a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,8 +22,6 @@ will consitute of a breaking change warranting a new major release: ## Changed - GPS Fix has changed event is no longer triggered for the EM - -## Changed - MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available. The stored value of the last timestep will now be reset, if no actual value is available.