From 281eb3237cb489bc8d29d4484d0b4f2f3128e936 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 17 Nov 2022 15:55:30 +0100
Subject: [PATCH] fixed susTotVec and mgmTotVec types to dataSet types

---
 mission/controller/acs/SensorProcessing.cpp | 29 ++++++++++++++-------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index 483b94c2..407ba900 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -115,13 +115,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
     }
   }
-  float mgmVecTot[3] = {0.0, 0.0, 0.0};
+  double mgmVecTot[3] = {0.0, 0.0, 0.0};
   for (uint8_t i = 0; i < 3; i++) {
     mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
   }
 
   //-----------------------Mgm Rate Computation ---------------------------------------------------
-  float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
+  double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool mgmVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
   if (timeOfSavedMagFieldEst.tv_sec != 0) {
@@ -367,7 +367,7 @@ void SensorProcessing::processSus(
                               sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
                               sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
 
-  float susMeanValue[3] = {0, 0, 0};
+  double susMeanValue[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 12; i++) {
     if (validIds[i]) {
       susMeanValue[0] += susVecBody[0][i];
@@ -375,12 +375,12 @@ void SensorProcessing::processSus(
       susMeanValue[2] += susVecBody[2][i];
     }
   }
-  float susVecTot[3] = {0.0, 0.0, 0.0};
-  VectorOperations<float>::normalize(susMeanValue, susVecTot, 3);
+  double susVecTot[3] = {0.0, 0.0, 0.0};
+  VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
 
   /* -------- Sun Derivatiative --------------------- */
 
-  float susVecTotDerivative[3] = {0.0, 0.0, 0.0};
+  double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool susVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
   if (timeOfSavedSusDirEst.tv_sec != 0) {
@@ -438,10 +438,10 @@ void SensorProcessing::processSus(
       susDataProcessed->sus10vec.setValid(sus10valid);
       std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
       susDataProcessed->sus11vec.setValid(sus11valid);
-      std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
       susDataProcessed->susVecTot.setValid(true);
       std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
-                  3 * sizeof(float));
+                  3 * sizeof(double));
       susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
       std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
       susDataProcessed->sunIjkModel.setValid(true);
@@ -538,9 +538,13 @@ void SensorProcessing::processGyr(
       std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
       gyrDataProcessed->gyr0vec.setValid(gyr0valid);
       std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
+      gyrDataProcessed->gyr1vec.setValid(gyr1valid);
       std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
+      gyrDataProcessed->gyr2vec.setValid(gyr2valid);
       std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
+      gyrDataProcessed->gyr3vec.setValid(gyr3valid);
       std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
+      gyrDataProcessed->gyrVecTot.setValid(true);
       gyrDataProcessed->setValidity(true, false);
     }
   }
@@ -582,7 +586,9 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
   processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
-             sensorValues->gpsSet.isValid(), gpsDataProcessed);
+             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
+              sensorValues->gpsSet.altitude.isValid()),
+             gpsDataProcessed);
 
   processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
              sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
@@ -594,7 +600,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
              sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
              sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
              now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
-             sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed);
+             sensorValues->gpsSet.altitude.value,
+             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
+              sensorValues->gpsSet.altitude.isValid()),
+             mgmDataProcessed);
 
   processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
              sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),