Merge pull request 'Fix Calculation of Fused Rotation Rate during Eclipse' (#768) from fused-rot-rate-fix into main

Reviewed-on: #768
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -24,6 +24,10 @@ will consitute of a breaking change warranting a new major release:
 - ACS Controller strategy is now actually written to the dataset for detumbling.
 - During detumble the fused rotation rate is now calculated.
 - Detumbling is now exited when its exit value is undercut and not its entry value.
+- Rotation rate of last cycle is now stored in all cases for the fused rotational rate
+  calculation.
+- Fused rotation rate estimation during eclipse can be disabled. This will also prevent
+  detumbling during eclipse, as no relevant rotational rate is available for now.
 - `EiveSystem`: Add a small delay between triggering an event for FDIR reboots and sending the
   command to the core controller.
 - PL PDU: Fixed bounds checking logic. Bound checks will only be performed for modules which are

mission/controller/acs/AcsParameters.cpp

@@ -26,6 +26,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x1:
           parameterWrapper->set(onBoardParams.mekfViolationTimer);
           break;
+        case 0x2:
+          parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -19,6 +19,7 @@ class AcsParameters : public HasParametersIF {
   struct OnBoardParams {
     double sampleTime = 0.4;  // [s]
     uint16_t mekfViolationTimer = 750;
+    uint8_t fusedRateSafeDuringEclipse = true;
   } onBoardParams;
 
   struct InertiaEIVE {

mission/controller/acs/FusedRotationEstimation.cpp

@@ -18,10 +18,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
       std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
       fusedRotRateData->setValidity(false, true);
     }
+    // store for calculation of angular acceleration
+    if (gyrDataProcessed->gyrVecTot.isValid()) {
+      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
+    }
     return;
   }
   if (not susDataProcessed->susVecTot.isValid()) {
     estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    // store for calculation of angular acceleration
+    if (gyrDataProcessed->gyrVecTot.isValid()) {
+      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
+    }
     return;
   }
 
@@ -42,6 +50,10 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
                                         fusedRotRateParallel, 3);
   } else {
     estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    // store for calculation of angular acceleration
+    if (gyrDataProcessed->gyrVecTot.isValid()) {
+      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
+    }
     return;
   }
 
@@ -58,11 +70,6 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
   double fusedRotRateTotal[3] = {0, 0, 0};
   VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
 
-  // store for calculation of angular acceleration
-  if (gyrDataProcessed->gyrVecTot.isValid()) {
-    std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
-  }
-
   {
     PoolReadGuard pg(fusedRotRateData);
     std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
@@ -71,11 +78,17 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
     std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
     fusedRotRateData->setValidity(true, true);
   }
+
+  // store for calculation of angular acceleration
+  if (gyrDataProcessed->gyrVecTot.isValid()) {
+    std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
+  }
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateEclipse(
     acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
-  if (not gyrDataProcessed->gyrVecTot.isValid() or
+  if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
+      not gyrDataProcessed->gyrVecTot.isValid() or
       VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
     {
       PoolReadGuard pg(fusedRotRateData);