eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Cleaner Calculation of MGM and SUS Rate #775

Merged
muellerr merged 7 commits from mgm-sus-rate-calc into main 2023-08-14 15:28:33 +02:00
Conversation 0 Commits 7 Files Changed 3 +30 -33
3 changed files with 30 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -28,6 +28,8 @@ will consitute of a breaking change warranting a new major release:
## Changed ## Changed
- GPS Fix has changed event is no longer triggered for the EM - GPS Fix has changed event is no longer triggered for the EM
- 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 # [v6.3.0] 2023-08-03

36
mission/controller/acs/SensorProcessing.cpp
View File

@ -1,19 +1,5 @@
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timevalOperations.h>
#include <math.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "Igrf13Model.h"
#include "util/MathOperations.h"
using namespace Math;
SensorProcessing::SensorProcessing() {} SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {} SensorProcessing::~SensorProcessing() {}
@ -54,6 +40,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgmDataProcessed->magIgrfModel.setValid(gpsValid); mgmDataProcessed->magIgrfModel.setValid(gpsValid);
} }
} }
std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot));
return; return;
} }
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
@ -141,14 +128,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false; bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) { if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
for (uint8_t i = 0; i < 3; i++) { VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
savedMgmVecTot[i] = mgmVecTot[i]; VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
mgmVecTotDerivativeValid = true; mgmVecTotDerivativeValid = true;
} }
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement; timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
mgmDataProcessed->mgmVecTotDerivative.isValid()) { mgmDataProcessed->mgmVecTotDerivative.isValid()) {
@ -277,6 +264,7 @@ void SensorProcessing::processSus(
susDataProcessed->sunIjkModel.setValid(true); susDataProcessed->sunIjkModel.setValid(true);
} }
} }
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
return; return;
} }
@ -365,13 +353,13 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false; bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) { if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
for (uint8_t i = 0; i < 3; i++) { VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
savedSusVecTot[i] = susVecTot[i]; VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
susVecTotDerivativeValid = true; susVecTotDerivativeValid = true;
} }
} std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
susDataProcessed->susVecTotDerivative.isValid()) { susDataProcessed->susVecTotDerivative.isValid()) {
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,

21
mission/controller/acs/SensorProcessing.h
View File

@ -1,15 +1,22 @@
#ifndef SENSORPROCESSING_H_ #ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_ #define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timevalOperations.h>
#include <fsfw/returnvalues/returnvalue.h> #include <fsfw/returnvalues/returnvalue.h>
#include <stdint.h> //uint8_t #include <mission/controller/acs/AcsParameters.h>
#include <time.h> /*purpose, timeval ?*/ #include <mission/controller/acs/Igrf13Model.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/SusConverter.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include <cmath>
#include "AcsParameters.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "eive/resultClassIds.h"
class SensorProcessing { class SensorProcessing {
public: public: