From dcf01d822b5ef37aacd9224c91e59c4187d4b5b7 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 19 Jun 2023 17:22:57 +0200
Subject: [PATCH] only check validity if sensor itself is valid

---
 mission/controller/acs/SensorProcessing.cpp | 55 +++++++++++++++------
 1 file changed, 40 insertions(+), 15 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index 35ca9736..d6c0d602 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -212,20 +212,45 @@ void SensorProcessing::processSus(
   sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
 
   uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-  susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
-  susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
-  susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
-  susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
-  susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
-  susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
-  susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
-  susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
-  susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
-  susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
-  susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
-  susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
+  if (sus0valid) {
+    susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
+  }
+  if (sus1valid) {
+    susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
+  }
+  if (sus2valid) {
+    susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
+  }
+  if (sus3valid) {
+    susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
+  }
+  if (sus4valid) {
+    susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
+  }
+  if (sus5valid) {
+    susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
+  }
+  if (sus6valid) {
+    susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
+  }
+  if (sus7valid) {
+    susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
+  }
+  if (sus8valid) {
+    susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
+  }
+  if (sus9valid) {
+    susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
+  }
+  if (sus10valid) {
+    susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
+  }
+  if (sus11valid) {
+    susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
+  }
 
-  bool susValid[12] = {true, true, true, true, true, true, true, true, true, true, true, true};
+  bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
+                       sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
   bool allInvalid =
       susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
 
@@ -246,8 +271,8 @@ void SensorProcessing::processSus(
         std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
         std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
         std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(double));
+        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(double));
         susDataProcessed->setValidity(false, true);
         std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
         susDataProcessed->sunIjkModel.setValid(true);