amended gyr sensor fusion

mission/controller/acs/AcsParameters.h

@@ -762,6 +762,8 @@ class AcsParameters /*: public HasParametersIF*/ {
                               pow(3.0e-3 * sqrt(2), 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(4.3e-3 * sqrt(2), 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)};
+    enum PreferAdis { NO = 0, YES = 1 };
+    uint8_t preferAdis = PreferAdis::YES;
   } gyrHandlingParameters;
 
   struct RwHandlingParameters {

mission/controller/acs/SensorProcessing.cpp

@@ -480,80 +480,57 @@ void SensorProcessing::processGyr(
   // Transforming Values to the Body Frame (actually it is the geometry frame atm)
   double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
          gyr3ValueBody[3] = {0, 0, 0};
+  float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
 
   if (gyr0valid) {
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
+    for (uint8_t i = 0; i < 3; i++) {
+      sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
+      sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
+    }
   }
   if (gyr1valid) {
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
+    for (uint8_t i = 0; i < 3; i++) {
+      sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
+      sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
+    }
   }
   if (gyr2valid) {
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
+    for (uint8_t i = 0; i < 3; i++) {
+      sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
+      sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
+    }
   }
   if (gyr3valid) {
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
+    for (uint8_t i = 0; i < 3; i++) {
+      sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
+      sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
+    }
   }
 
   /* -------- SatRateEst: Middle Value ------- */
   // take ADIS measurements, if both avail
   // if just one ADIS measurement avail, perform sensor fusion
   double gyrVecTot[3] = {0.0, 0.0, 0.0};
-  if (gyr0valid && gyr2valid) {
+  if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
     double gyr02ValuesSum[3];
     VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
     VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
-  } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) {
-    if (gyr0valid) {
-      std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double));
-    } else if (gyr2valid) {
-      std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double));
-    }
-  } else if (gyr1valid && gyr3valid) {
-    double gyr13ValuesSum[3];
-    double gyr13ValuesMean[3];
-    VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
-    VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
-    if (gyr0valid) {
-      gyrVecTot[0] =
-          ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
-           (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
-          ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
-      gyrVecTot[1] =
-          ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
-           (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
-          ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
-      gyrVecTot[2] =
-          ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
-           (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
-          ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
-    } else if (gyr2valid) {
-      gyrVecTot[0] =
-          ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
-           (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
-          ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
-      gyrVecTot[1] =
-          ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
-           (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
-          ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
-      gyrVecTot[2] =
-          ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
-           (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
-          ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
   } else {
-      std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double));
+    for (uint8_t i = 0; i < 3; i++) {
+      gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
     }
-  } else if (gyr1valid) {
-    std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double));
-  } else if (gyr3valid) {
-    std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double));
   }
   {
     PoolReadGuard pg(gyrDataProcessed);