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 {

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<double>::mulScalar(newValue, (1 - weight), leftSide, 3);
+  VectorOperations<double>::mulScalar(oldValue, weight, rightSide, 3);
+  VectorOperations<double>::add(leftSide, rightSide, newValue, 3);
+}

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