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

added low pass filter for gyr and mgmDot values
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-04-05 11:25:44 +02:00
parent 4ca8c38c98
commit 7a7d0e650f
3 changed files with 21 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -150,6 +150,11 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
if (mgmDataProcessed->mgmVecTotDerivative.isValid()) {
lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value,
mgmParameters->mgmDerivativeFilterWeight);
}
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -527,6 +532,11 @@ void SensorProcessing::processGyr(
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
}
if (gyrDataProcessed->gyrVecTot.isValid()) {
lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight);
}
{
PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -656,3 +666,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
}
void SensorProcessing::lowPassFilter(double *newValue, double *oldValue, const double weight) {
double leftSide[3] = {0, 0, 0}, rightSide[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(newValue, (1 - weight), leftSide, 3);
VectorOperations<double>::mulScalar(oldValue, weight, rightSide, 3);
VectorOperations<double>::add(leftSide, rightSide, newValue, 3);
}