diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 92de01e0..bc78e34d 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -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 { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index b2812f55..15e7767c 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -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::mulScalar(newValue, (1 - weight), leftSide, 3); + VectorOperations::mulScalar(oldValue, weight, rightSide, 3); + VectorOperations::add(leftSide, rightSide, newValue, 3); +} diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index d845c2f3..47449a75 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -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};