eive/eive-obsw
eive/eive-obsw
12
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 119 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/acs/AcsParameters.h
View File

@ -72,6 +72,7 @@ class AcsParameters : public HasParametersIF {
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmDerivativeFilterWeight = 0.5;
} mgmHandlingParameters;
struct SusHandlingParameters {
@ -780,6 +781,7 @@ class AcsParameters : public HasParametersIF {
pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = true;
float gyrFilterWeight = 0.6;
} gyrHandlingParameters;
struct RwHandlingParameters {

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);
}

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

@ -13,8 +13,6 @@
class SensorProcessing {
public:
void reset();
SensorProcessing();
virtual ~SensorProcessing();
@ -59,13 +57,13 @@ class SensorProcessing {
const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed);
void processStr();
void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
const double gpsUnixSeconds, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed);
void lowPassFilter(double *newValue, double *oldValue, const double weight);
double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3] = {0.0, 0.0, 0.0};