From 9089569b93876ffc4ef34ad0ca287cd161eb2cfa Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 18 Dec 2023 09:33:46 +0100
Subject: [PATCH] quat is always valid even if it shouldn't be

---
 mission/controller/AcsController.cpp               | 6 ++++--
 mission/controller/acs/FusedRotationEstimation.cpp | 6 ++++--
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 6adb4a0b..54d96699 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -380,8 +380,10 @@ void AcsController::performDetumble() {
 }
 
 void AcsController::performPointingCtrl() {
-  bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
-                   sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
+  bool strValid =
+      ((sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
+        sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()) and
+       (sensorValues.strSet.isTrustWorthy.isValid() and sensorValues.strSet.isTrustWorthy.value));
   uint8_t useMekf = false;
   switch (mode) {
     case acs::PTG_IDLE:
diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp
index b4d0f0c4..758004d2 100644
--- a/mission/controller/acs/FusedRotationEstimation.cpp
+++ b/mission/controller/acs/FusedRotationEstimation.cpp
@@ -73,8 +73,10 @@ void FusedRotationEstimation::estimateFusedRotationRate(
 void FusedRotationEstimation::estimateFusedRotationRateStr(
     ACS::SensorValues *sensorValues, const double timeDelta,
     acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
-          sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
+  if (not((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
+           sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) and
+          (sensorValues->strSet.isTrustWorthy.isValid() and
+           sensorValues->strSet.isTrustWorthy.value))) {
     {
       PoolReadGuard pg(fusedRotRateSourcesData);
       if (pg.getReadResult() == returnvalue::OK) {