mission/controller/AcsController.cpp

@@ -95,7 +95,8 @@ void AcsController::performSafe() {
   timeval now;
   Clock::getClock_timeval(&now);
 
-  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
   navigation.useMekf(&sensorValues, &outputValues, &validMekf);
 
@@ -148,7 +149,8 @@ void AcsController::performDetumble() {
   timeval now;
   Clock::getClock_timeval(&now);
 
-  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
   navigation.useMekf(&sensorValues, &outputValues, &validMekf);
 
@@ -182,7 +184,8 @@ void AcsController::performPointingCtrl() {
   timeval now;
   Clock::getClock_timeval(&now);
 
-  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   ReturnValue_t validMekf;
   navigation.useMekf(&sensorValues, &outputValues, &validMekf);
 

mission/controller/acs/SensorProcessing.cpp

@@ -1,5 +1,6 @@
 #include "SensorProcessing.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
@@ -13,27 +14,35 @@
 
 using namespace Math;
 
-SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} {
+SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
-  validMagField = false;
+    : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
-  validGcLatitude = false;
-}
 
 SensorProcessing::~SensorProcessing() {}
 
-bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
+void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
                                   bool mgm1valid, const float *mgm2Value, bool mgm2valid,
                                   const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
                                   bool mgm4valid, timeval timeOfMgmMeasurement,
                                   const AcsParameters::MgmHandlingParameters *mgmParameters,
-                                  const double gpsLatitude, const double gpsLongitude,
+                                  acsctrl::GpsDataProcessed *gpsDataProcessed,
-                                  const double gpsAltitude, bool gpsValid, double *magFieldEst,
+                                  const double gpsAltitude, bool gpsValid,
-                                  bool *outputValid, double *magFieldModel,
+                                  acsctrl::MgmDataProcessed *mgmDataProcessed) {
-                                  bool *magFieldModelValid, double *magneticFieldVectorDerivative,
-                                  bool *magneticFieldVectorDerivativeValid) {
   if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
-    *outputValid = false;
+    {
-    validMagField = false;
+      PoolReadGuard pg(mgmDataProcessed);
-    return false;
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
+        mgmDataProcessed->setValidity(false, true);
+      }
+    }
+    return;
   }
   float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
         mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
@@ -104,34 +113,26 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
     }
   }
+  float mgmVecTot[3] = {0.0, 0.0, 0.0};
   for (uint8_t i = 0; i < 3; i++) {
-    magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
+    mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
   }
-  validMagField = true;
 
-  //-----------------------Mag Rate Computation ---------------------------------------------------
+  //-----------------------Mgm Rate Computation ---------------------------------------------------
+  float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
+  bool mgmVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
+  if (timeOfSavedMagFieldEst.tv_sec != 0) {
     for (uint8_t i = 0; i < 3; i++) {
-    magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff;
+      mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
-    savedMagFieldEst[i] = magFieldEst[i];
+      savedMgmVecTot[i] = mgmVecTot[i];
     }
-
-  *magneticFieldVectorDerivativeValid = true;
-  if (timeOfSavedMagFieldEst.tv_sec == 0) {
-    magneticFieldVectorDerivative[0] = 0;
-    magneticFieldVectorDerivative[1] = 0;
-    magneticFieldVectorDerivative[2] = 0;
-    *magneticFieldVectorDerivativeValid = false;
   }
-
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
 
-  *outputValid = true;
-
   // ---------------- IGRF- 13 Implementation here ------------------------------------------------
-  if (!gpsValid) {
+  double magIgrfModel[3] = {0.0, 0.0, 0.0};
-    *magFieldModelValid = false;
+  if (gpsValid) {
-  } else {
     // Should be existing class object which will be called and modified here.
     Igrf13Model igrf13;
     // So the line above should not be done here. Update: Can be done here as long updated coffs
@@ -139,12 +140,32 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
     igrf13.updateCoeffGH(timeOfMgmMeasurement);
     // maybe put a condition here, to only update after a full day, this
     // class function has around 700 steps to perform
-    igrf13.magFieldComp(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement,
+    igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
-                        magFieldModel);
+                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
-    *magFieldModelValid = false;
+  }
+  {
+    PoolReadGuard pg(mgmDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
+      mgmDataProcessed->mgm0vec.setValid(mgm0valid);
+      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
+      mgmDataProcessed->mgm1vec.setValid(mgm1valid);
+      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
+      mgmDataProcessed->mgm2vec.setValid(mgm2valid);
+      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
+      mgmDataProcessed->mgm3vec.setValid(mgm3valid);
+      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
+      mgmDataProcessed->mgm4vec.setValid(mgm4valid);
+      std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(float));
+      mgmDataProcessed->mgmVecTot.setValid(true);
+      std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
+                  3 * sizeof(float));
+      mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
+      std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
+      mgmDataProcessed->magIgrfModel.setValid(gpsValid);
+      mgmDataProcessed->setValidity(true, false);
+    }
   }
-
-  return true;
 }
 
 void SensorProcessing::processSus(
@@ -155,9 +176,8 @@ void SensorProcessing::processSus(
     const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
     const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
     timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
-    const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
+    const AcsParameters::SunModelParameters *sunModelParameters,
-    bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
+    acsctrl::SusDataProcessed *susDataProcessed) {
-    double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
   if (sus0valid) {
     sus0valid = susConverter.checkSunSensorData(sus0Value);
   }
@@ -197,9 +217,29 @@ void SensorProcessing::processSus(
 
   if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
       !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
-    *sunDirEstValid = false;
+    {
+      PoolReadGuard pg(susDataProcessed);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
+        susDataProcessed->setValidity(false, true);
+      }
+    }
     return;
-  } else {
+  }
   // WARNING: NOT TRANSFORMED IN BODY FRAME YET
   // Transformation into Geomtry Frame
   float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
@@ -214,6 +254,16 @@ void SensorProcessing::processSus(
                                              susParameters->sus0coeffBeta),
         sus0VecBody, 3, 3, 1);
   }
+  {
+    PoolReadGuard pg(susDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
+      susDataProcessed->sus0vec.setValid(sus0valid);
+      if (!sus0valid) {
+        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
+      }
+    }
+  }
   if (sus1valid) {
     MatrixOperations<float>::multiply(
         susParameters->sus1orientationMatrix[0],
@@ -221,6 +271,16 @@ void SensorProcessing::processSus(
                                              susParameters->sus1coeffBeta),
         sus1VecBody, 3, 3, 1);
   }
+  {
+    PoolReadGuard pg(susDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
+      susDataProcessed->sus1vec.setValid(sus1valid);
+      if (!sus1valid) {
+        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
+      }
+    }
+  }
   if (sus2valid) {
     MatrixOperations<float>::multiply(
         susParameters->sus2orientationMatrix[0],
@@ -305,7 +365,7 @@ void SensorProcessing::processSus(
                               sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
                               sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
 
-    double susMeanValue[3] = {0, 0, 0};
+  float susMeanValue[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 12; i++) {
     if (validIds[i]) {
       susMeanValue[0] += susVecBody[0][i];
@@ -313,26 +373,20 @@ void SensorProcessing::processSus(
       susMeanValue[2] += susVecBody[2][i];
     }
   }
-    VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
+  float susVecTot[3] = {0.0, 0.0, 0.0};
-    *sunDirEstValid = true;
+  VectorOperations<float>::normalize(susMeanValue, susVecTot, 3);
-  }
 
   /* -------- Sun Derivatiative --------------------- */
 
+  float susVecTotDerivative[3] = {0.0, 0.0, 0.0};
+  bool susVecTotDerivativeValid = false;
   double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
+  if (timeOfSavedSusDirEst.tv_sec != 0) {
     for (uint8_t i = 0; i < 3; i++) {
-    sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff;
+      susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
-    savedSunVector[i] = sunDirEst[i];
+      savedSusVecTot[i] = susVecTot[i];
     }
-
-  *sunVectorDerivativeValid = true;
-  if (timeOfSavedSusDirEst.tv_sec == 0) {
-    sunVectorDerivative[0] = 0;
-    sunVectorDerivative[1] = 0;
-    sunVectorDerivative[2] = 0;
-    *sunVectorDerivativeValid = false;
   }
-
   timeOfSavedSusDirEst = timeOfSusMeasurement;
 
   /* -------- Sun Model Direction (IJK frame) ------- */
@@ -340,6 +394,7 @@ void SensorProcessing::processSus(
   double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
 
   // Julean Centuries
+  double sunIjkModel[3] = {0.0, 0.0, 0.0};
   double JC2000 = JD2000 / 36525;
 
   double meanLongitude =
@@ -351,11 +406,46 @@ void SensorProcessing::processSus(
 
   double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
 
-  sunVectorInertial[0] = cos(eclipticLongitude);
+  sunIjkModel[0] = cos(eclipticLongitude);
-  sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon);
+  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
-  sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon);
+  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
-
+  {
-  *sunVectorInertialValid = true;
+    PoolReadGuard pg(susDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
+      susDataProcessed->sus0vec.setValid(sus0valid);
+      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
+      susDataProcessed->sus1vec.setValid(sus1valid);
+      std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
+      susDataProcessed->sus2vec.setValid(sus2valid);
+      std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
+      susDataProcessed->sus3vec.setValid(sus3valid);
+      std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
+      susDataProcessed->sus4vec.setValid(sus4valid);
+      std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
+      susDataProcessed->sus5vec.setValid(sus5valid);
+      std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
+      susDataProcessed->sus6vec.setValid(sus6valid);
+      std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
+      susDataProcessed->sus7vec.setValid(sus7valid);
+      std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
+      susDataProcessed->sus8vec.setValid(sus8valid);
+      std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
+      susDataProcessed->sus9vec.setValid(sus9valid);
+      std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
+      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));
+      susDataProcessed->susVecTot.setValid(true);
+      std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
+                  3 * sizeof(float));
+      susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
+      std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
+      susDataProcessed->sunIjkModel.setValid(true);
+      susDataProcessed->setValidity(true, false);
+    }
+  }
 }
 
 void SensorProcessing::processGyr(
@@ -366,133 +456,166 @@ void SensorProcessing::processGyr(
     const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
     const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
     timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
-    double *satRatEst, bool *satRateEstValid) {
+    acsctrl::GyrDataProcessed *gyrDataProcessed) {
-  if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid &&
+  bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
-      !gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid &&
+  bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
-      !gyr3axYvalid && !gyr3axZvalid) {
+  bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid);
-    *satRateEstValid = false;
+  bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid);
+  if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) {
+    {
+      PoolReadGuard pg(gyrDataProcessed);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
+        gyrDataProcessed->setValidity(false, true);
+      }
+    }
     return;
   }
   // Transforming Values to the Body Frame (actually it is the geometry frame atm)
   double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
          gyr3ValueBody[3] = {0, 0, 0};
 
-  bool validUnit[4] = {false, false, false, false};
+  if (gyr0valid) {
-  if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
-    validUnit[0] = true;
   }
-  if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
+  if (gyr1valid) {
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
-    validUnit[1] = true;
   }
-  if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
+  if (gyr2valid) {
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
-    validUnit[2] = true;
   }
-  if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
+  if (gyr3valid) {
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
-    validUnit[3] = true;
   }
 
   /* -------- SatRateEst: Middle Value ------- */
   // take ADIS measurements, if both avail
   // if just one ADIS measurement avail, perform sensor fusion
-  if (validUnit[0] && validUnit[2]) {
+  double gyrVecTot[3] = {0.0, 0.0, 0.0};
+  if (gyr0valid && gyr2valid) {
     double gyr02ValuesSum[3];
     VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
-    VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, satRatEst, 3);
+    VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
-  } else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) {
+  } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) {
-    if (validUnit[0]) {
+    if (gyr0valid) {
-      satRatEst = gyr0ValueBody;
+      std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double));
-    } else if (validUnit[2]) {
+    } else if (gyr2valid) {
-      satRatEst = gyr2ValueBody;
+      std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double));
     }
-  } else if ((validUnit[1]) && (validUnit[3])) {
+  } else if (gyr1valid && gyr3valid) {
     double gyr13ValuesSum[3];
     double gyr13ValuesMean[3];
     VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
     VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
-    if (validUnit[0]) {
+    if (gyr0valid) {
-      satRatEst[0] =
+      gyrVecTot[0] =
           ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
            (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
           ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
-      satRatEst[1] =
+      gyrVecTot[1] =
           ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
            (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
           ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
-      satRatEst[2] =
+      gyrVecTot[2] =
           ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
            (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
           ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
-    } else if (validUnit[2]) {
+    } else if (gyr2valid) {
-      satRatEst[0] =
+      gyrVecTot[0] =
           ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
            (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
           ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
-      satRatEst[1] =
+      gyrVecTot[1] =
           ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
            (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
           ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
-      satRatEst[2] =
+      gyrVecTot[2] =
           ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
            (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
           ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
-    } else
+    } else {
-      satRatEst = gyr13ValuesMean;
+      std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double));
-  } else if (validUnit[1]) {
+    }
-    satRatEst = gyr1ValueBody;
+  } else if (gyr1valid) {
-  } else if (validUnit[3]) {
+    std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double));
-    satRatEst = gyr3ValueBody;
+  } else if (gyr3valid) {
+    std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double));
+  }
+  {
+    PoolReadGuard pg(gyrDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
+      gyrDataProcessed->gyr0vec.setValid(gyr0valid);
+      std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
+      std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
+      std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
+      std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
+      gyrDataProcessed->setValidity(true, false);
+    }
   }
-  *satRateEstValid = true;
 }
 
 void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
-                                  const bool validGps, double *gcLatitude, double *gdLongitude) {
+                                  const bool validGps,
+                                  acsctrl::GpsDataProcessed *gpsDataProcessed) {
   // name to convert not process
+  double gdLongitude, gcLatitude;
   if (validGps) {
     // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
-    *gdLongitude = gps0longitude * PI / 180;
+    gdLongitude = gps0longitude * PI / 180;
     double latitudeRad = gps0latitude * PI / 180;
     double eccentricityWgs84 = 0.0818195;
     double factor = 1 - pow(eccentricityWgs84, 2);
-    *gcLatitude = atan(factor * tan(latitudeRad));
+    gcLatitude = atan(factor * tan(latitudeRad));
-    validGcLatitude = true;
+    // validGcLatitude = true;
+  }
+  {
+    PoolReadGuard pg(gpsDataProcessed);
+    if (pg.getReadResult() == returnvalue::OK) {
+      gpsDataProcessed->gdLongitude.value = gdLongitude;
+      gpsDataProcessed->gcLatitude.value = gcLatitude;
+      gpsDataProcessed->setValidity(validGps, validGps);
+      if (!validGps) {
+        gpsDataProcessed->gdLongitude.value = 0.0;
+        gpsDataProcessed->gcLatitude.value = 0.0;
+      }
+    }
   }
 }
 
 void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
-                               ACS::OutputValues *outputValues,
+                               acsctrl::MgmDataProcessed *mgmDataProcessed,
+                               acsctrl::SusDataProcessed *susDataProcessed,
+                               acsctrl::GyrDataProcessed *gyrDataProcessed,
+                               acsctrl::GpsDataProcessed *gpsDataProcessed,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
   processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
-             sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude);
+             sensorValues->gpsSet.isValid(), gpsDataProcessed);
 
-  outputValues->mgmUpdated = processMgm(
+  processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
-      sensorValues->mgm0Lis3Set.fieldStrengths.value,
              sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
              sensorValues->mgm1Rm3100Set.fieldStrengths.value,
              sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
              sensorValues->mgm2Lis3Set.fieldStrengths.value,
              sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
              sensorValues->mgm3Rm3100Set.fieldStrengths.value,
-      sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
+             sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
-      sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
+             sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
-      outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
+             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
-      sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
+             sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed);
-      outputValues->magFieldModel, &outputValues->magFieldModelValid,
-      outputValues->magneticFieldVectorDerivative,
-      &outputValues->magneticFieldVectorDerivativeValid);  // VALID outputs- PoolVariable ?
 
   processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
              sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@@ -507,10 +630,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
              sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
              sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
              now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
-             outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel,
+             susDataProcessed);
-             &outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
-             &outputValues->sunVectorDerivativeValid);
-  // VALID outputs ?
 
   processGyr(
       sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@@ -525,6 +645,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
       sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
       sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
       sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
-      &acsParameters->gyrHandlingParameters, outputValues->satRateEst,
+      &acsParameters->gyrHandlingParameters, gyrDataProcessed);
-      &outputValues->satRateEstValid);
 }

mission/controller/acs/SensorProcessing.h

@@ -23,19 +23,21 @@ class SensorProcessing {
   SensorProcessing(AcsParameters *acsParameters_);
   virtual ~SensorProcessing();
 
-  void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+  void process(timeval now, ACS::SensorValues *sensorValues,
+               acsctrl::MgmDataProcessed *mgmDataProcessed,
+               acsctrl::SusDataProcessed *susDataProcessed,
+               acsctrl::GyrDataProcessed *gyrDataProcessed,
+               acsctrl::GpsDataProcessed *gpsDataProcessed,
                const AcsParameters *acsParameters);  // Will call protected functions
  private:
  protected:
   // short description needed for every function
-  bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
+  void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
                   const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
                   const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
                   const AcsParameters::MgmHandlingParameters *mgmParameters,
-                  const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
+                  acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
-                  bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel,
+                  bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
-                  bool *magFieldModelValid, double *magneticFieldVectorDerivative,
-                  bool *magneticFieldVectorDerivativeValid);  // Output
 
   void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
                   bool sus1valid, const uint16_t *sus2Value, bool sus2valid,
@@ -47,9 +49,8 @@ class SensorProcessing {
                   bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
                   timeval timeOfSusMeasurement,
                   const AcsParameters::SusHandlingParameters *susParameters,
-                  const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
+                  const AcsParameters::SunModelParameters *sunModelParameters,
-                  bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
+                  acsctrl::SusDataProcessed *susDataProcessed);
-                  double *sunVectorDerivative, bool *sunVectorDerivativeValid);
 
   void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue,
                   bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid,
@@ -60,21 +61,23 @@ class SensorProcessing {
                   const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
                   bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
                   timeval timeOfGyrMeasurement,
-                  const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst,
+                  const AcsParameters::GyrHandlingParameters *gyrParameters,
-                  bool *satRateEstValid);
+                  acsctrl::GyrDataProcessed *gyrDataProcessed);
 
   void processStr();
 
   void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
-                  double *gcLatitude, double *gdLongitude);
+                  acsctrl::GpsDataProcessed *gpsDataProcessed);
 
-  double savedMagFieldEst[3];
+  double savedMgmVecTot[3];
   timeval timeOfSavedMagFieldEst;
-  double savedSunVector[3];
+  double savedSusVecTot[3];
   timeval timeOfSavedSusDirEst;
   bool validMagField;
   bool validGcLatitude;
 
+  const float zeroVector[3] = {0.0, 0.0, 0.0};
+
   SusConverter susConverter;
   AcsParameters acsParameters;
 };