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
- 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

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

@ -1,19 +1,5 @@
#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() {}
@ -54,6 +40,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 +128,14 @@ 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];
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
mgmVecTotDerivativeValid = true;
}
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
mgmDataProcessed->mgmVecTotDerivative.isValid()) {
@ -277,6 +264,7 @@ void SensorProcessing::processSus(
susDataProcessed->sunIjkModel.setValid(true);
}
}
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
return;
}
@ -365,13 +353,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];
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
susVecTotDerivativeValid = true;
}
}
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
susDataProcessed->susVecTotDerivative.isValid()) {
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,

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

@ -1,15 +1,22 @@
#ifndef 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 <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include <mission/controller/acs/AcsParameters.h>
#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 "AcsParameters.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "eive/resultClassIds.h"
#include <cmath>
class SensorProcessing {
public: