Merge pull request 'Small ACS Fixes' (#666) from small-acs-fixes into v3.0.0-dev

Reviewed-on: #666

CHANGELOG.md

@@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release:
   reason.
 - OFF mode is ignores in TM store for determining whether a store will be written. The modes will
   only be used to cancel a transfer.
+- Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter
+  commands.
 
 ## Added
 
@@ -91,6 +93,8 @@ will consitute of a breaking change warranting a new major release:
 - Prevent spam of TCS controller heater unavailability event if all heaters are in external control.
 - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS
   controller. There is not crash risk but the heater states were invalid.
+- Fixed usage of quaternion valid flag, which does not actually represent the validity of the
+  quaternion.
 
 # [v2.0.5] 2023-05-11
 

mission/controller/AcsController.cpp

@@ -349,8 +349,10 @@ void AcsController::performPointingCtrl() {
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    if (multipleRwUnavailableCounter == 5) {
+    if (multipleRwUnavailableCounter >=
+        acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
+      multipleRwUnavailableCounter = 0;
     }
     multipleRwUnavailableCounter++;
     return;

mission/controller/acs/AcsParameters.cpp

@@ -290,6 +290,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x6:
           parameterWrapper->set(rwHandlingParameters.rampTime);
           break;
+        case 0x7:
+          parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -798,6 +798,8 @@ class AcsParameters : public HasParametersIF {
     double stictionTorque = 0.0006;
 
     uint16_t rampTime = 10;
+
+    uint32_t multipleRwInvalidTimeout = 25;
   } rwHandlingParameters;
 
   struct RwMatrices {

mission/controller/acs/Navigation.cpp

@@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                   acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
   double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
-                     sensorValues->strSet.caliQy.isValid() &&
-                     sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
+  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
     mekfStatus = multiplicativeKalmanFilter.init(