mission/controller/acs/AcsParameters.cpp

@@ -226,6 +226,9 @@ AcsParameters::~AcsParameters() {}
           case 0x3:
             parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
             break;
+          case 0x4:
+            parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
+            break;
           default:
             return INVALID_IDENTIFIER_ID;
         }

mission/controller/acs/AcsParameters.h

@@ -753,6 +753,7 @@ class AcsParameters /*: public HasParametersIF*/ {
     double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
     double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
     double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
+    float gyrFusionWeight = 0.8;
   } gyrHandlingParameters;
 
   struct RwHandlingParameters {

mission/controller/acs/SensorProcessing.cpp

@@ -1,10 +1,3 @@
-/*
- * SensorProcessing.cpp
- *
- *  Created on: 7 Mar 2022
- *      Author: Robin Marquardt
- */
-
 #include "SensorProcessing.h"
 
 #include <fsfw/globalfunctions/constants.h>
@@ -397,59 +390,77 @@ void SensorProcessing::processGyr(
          gyr3ValueBody[3] = {0, 0, 0};
 
   bool validUnit[4] = {false, false, false, false};
-  uint8_t validCount = 0;
   if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
-    validCount += 1;
     validUnit[0] = true;
   }
   if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
-    validCount += 1;
     validUnit[1] = true;
   }
   if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
-    validCount += 1;
     validUnit[2] = true;
   }
   if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
-    validCount += 1;
     validUnit[3] = true;
   }
 
   /* -------- SatRateEst: Middle Value ------- */
-  double gyrValues[3][4] = {
-      {gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]},
-      {gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]},
-      {gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}};
-  double gyrValidValues[3][validCount];
-  uint8_t j = 0;
-  for (uint8_t i = 0; i < validCount; i++) {
-    if (validUnit[i]) {
-      gyrValidValues[0][j] = gyrValues[0][i];
-      gyrValidValues[1][j] = gyrValues[1][i];
-      gyrValidValues[2][j] = gyrValues[2][i];
-      j += 1;
+  // take ADIS measurements, if both avail
+  // if just one ADIS measurement avail, perform sensor fusion
+  if (validUnit[0] && validUnit[2]) {
+    double gyr02ValuesSum[3];
+    VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
+    VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, satRatEst, 3);
+  } else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) {
+    if (validUnit[0]) {
+      satRatEst = gyr0ValueBody;
+    } else if (validUnit[2]) {
+      satRatEst = gyr2ValueBody;
     }
-  }
-  // Selection Sort
-  double gyrValidValuesSort[3][validCount];
-  MathOperations<double>::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount);
+  } else if ((validUnit[1]) && (validUnit[3])) {
+    double gyr13ValuesSum[3];
+    double gyr13ValuesMean[3];
+    VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
+    VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
+    if (validUnit[0]) {
+      satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[0] +
+                     gyrParameters->gyrFusionWeight * gyr0ValueBody[0];
+      satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[1] +
+                     gyrParameters->gyrFusionWeight * gyr0ValueBody[1];
+      satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[2] +
+                     gyrParameters->gyrFusionWeight * gyr0ValueBody[2];
 
-  uint8_t n = ceil(validCount / 2);
-  satRatEst[0] = gyrValidValuesSort[0][n];
-  satRatEst[1] = gyrValidValuesSort[1][n];
-  satRatEst[2] = gyrValidValuesSort[2][n];
+    } else if (validUnit[2]) {
+      satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[0] +
+                     gyrParameters->gyrFusionWeight * gyr2ValueBody[0];
+      satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[1] +
+                     gyrParameters->gyrFusionWeight * gyr2ValueBody[1];
+      satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
+                         gyr13ValuesMean[2] +
+                     gyrParameters->gyrFusionWeight * gyr2ValueBody[2];
+    } else
+      satRatEst = gyr13ValuesMean;
+  } else if (validUnit[1]) {
+    satRatEst = gyr1ValueBody;
+  } else if (validUnit[3]) {
+    satRatEst = gyr3ValueBody;
+  }
   *satRateEstValid = true;
 }