Merge pull request 'Parameter for STR for MEKF' (#877) from no-str-for-mekf into main

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

CHANGELOG.md

@@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+## Added
+
+- Added parameter to disable STR input for MEKF.
+
 # [v7.7.2] 2024-03-06
 
 ## Fixed

mission/controller/AcsController.cpp

@@ -180,7 +180,8 @@ void AcsController::performAttitudeControl() {
       mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
       &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, timeDelta, &attitudeEstimationData);
+                              &susDataProcessed, timeDelta, &attitudeEstimationData,
+                              acsParameters.kalmanFilterParameters.allowStr);
 
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {

mission/controller/acs/AcsParameters.cpp

@@ -734,6 +734,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x5:
           parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
           break;
+        case 0x6:
+          parameterWrapper->set(kalmanFilterParameters.allowStr);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -947,6 +947,8 @@ class AcsParameters : public HasParametersIF {
 
     double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD;  // Angular Random Walk
     double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD;               // Bias Stability
+
+    uint8_t allowStr = false;
   } kalmanFilterParameters;
 
   struct MagnetorquerParameter {

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -626,11 +626,11 @@ void MultiplicativeKalmanFilter::updateDataSet(
   }
 }
 
-void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW,
+void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, bool valid,
-                                            bool valid) {
+                                            bool allowStr) {
   strData.strQuat.value[0] = qX;
   strData.strQuat.value[1] = qY;
   strData.strQuat.value[2] = qZ;
   strData.strQuat.value[3] = qW;
-  strData.strQuat.valid = valid;
+  strData.strQuat.valid = (valid and allowStr);
 }

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -38,7 +38,7 @@ class MultiplicativeKalmanFilter {
   void updateStandardDeviations(const AcsParameters *acsParameters);
 
   void setStrData(const double qX, const double qY, const double qZ, const double qW,
-                  const bool valid);
+                  const bool valid, const bool allowStr);
 
   static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
   static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);

mission/controller/acs/Navigation.cpp

@@ -9,11 +9,12 @@ ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
                                   const acsctrl::MgmDataProcessed *mgmDataProcessed,
                                   const acsctrl::SusDataProcessed *susDataProcessed,
                                   const double timeDelta,
-                                  acsctrl::AttitudeEstimationData *attitudeEstimationData) {
+                                  acsctrl::AttitudeEstimationData *attitudeEstimationData,
+                                  const bool allowStr) {
   multiplicativeKalmanFilter.setStrData(
       sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
-      sensorValues->strSet.caliQx.isValid());
+      sensorValues->strSet.caliQx.isValid(), allowStr);
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
     mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,

mission/controller/acs/Navigation.h

@@ -17,7 +17,8 @@ class Navigation {
                         const acsctrl::GyrDataProcessed *gyrDataProcessed,
                         const acsctrl::MgmDataProcessed *mgmDataProcessed,
                         const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
-                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
+                        acsctrl::AttitudeEstimationData *attitudeEstimationData,
+                        const bool allowStr);
   void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);