changed sun and mgm model calc to always be executed

mission/controller/acs/SensorProcessing.cpp

@@ -33,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
                                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                                   const double gpsAltitude, bool gpsValid,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed) {
+  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
+  double magIgrfModel[3] = {0.0, 0.0, 0.0};
+  if (gpsValid) {
+    // 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
+    //		stored in acsParameters ?
+    igrf13.schmidtNormalization();
+    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(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
+                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
+  }
   if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
     {
       PoolReadGuard pg(mgmDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
+        float zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
         mgmDataProcessed->setValidity(false, true);
+        std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
+        mgmDataProcessed->magIgrfModel.setValid(gpsValid);
       }
     }
     return;
@@ -138,20 +154,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   }
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
 
-  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
-  double magIgrfModel[3] = {0.0, 0.0, 0.0};
-  if (gpsValid) {
-    // 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
-    //		stored in acsParameters ?
-    igrf13.schmidtNormalization();
-    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(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
-                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
-  }
   {
     PoolReadGuard pg(mgmDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -187,6 +189,25 @@ void SensorProcessing::processSus(
     timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
     const AcsParameters::SunModelParameters *sunModelParameters,
     acsctrl::SusDataProcessed *susDataProcessed) {
+  /* -------- Sun Model Direction (IJK frame) ------- */
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
+
+  // Julean Centuries
+  double sunIjkModel[3] = {0.0, 0.0, 0.0};
+  double JC2000 = JD2000 / 36525.;
+
+  double meanLongitude =
+      sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
+  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
+
+  double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
+                             sunModelParameters->p2 * sin(2 * meanAnomaly);
+
+  double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
+
+  sunIjkModel[0] = cos(eclipticLongitude);
+  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
+  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
   if (sus0valid) {
     sus0valid = susConverter.checkSunSensorData(sus0Value);
   }
@@ -229,22 +250,24 @@ void SensorProcessing::processSus(
     {
       PoolReadGuard pg(susDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
+        float zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
         susDataProcessed->setValidity(false, true);
+        std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
+        susDataProcessed->sunIjkModel.setValid(true);
       }
     }
     return;
@@ -263,16 +286,6 @@ 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],
@@ -280,16 +293,6 @@ 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],
@@ -397,27 +400,6 @@ void SensorProcessing::processSus(
     }
   }
   timeOfSavedSusDirEst = timeOfSusMeasurement;
-
-  /* -------- Sun Model Direction (IJK frame) ------- */
-  // if (useSunModel) eventuell
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
-
-  // Julean Centuries
-  double sunIjkModel[3] = {0.0, 0.0, 0.0};
-  double JC2000 = JD2000 / 36525.;
-
-  double meanLongitude =
-      sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
-  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
-
-  double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
-                             sunModelParameters->p2 * sin(2 * meanAnomaly);
-
-  double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
-
-  sunIjkModel[0] = cos(eclipticLongitude);
-  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
-  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
   {
     PoolReadGuard pg(susDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -474,6 +456,7 @@ void SensorProcessing::processGyr(
     {
       PoolReadGuard pg(gyrDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
+        double zeroVector[3] = {0.0, 0.0, 0.0};
         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));

mission/controller/acs/SensorProcessing.h

@@ -79,7 +79,6 @@ class SensorProcessing {
   double savedPosSatE[3] = {0.0, 0.0, 0.0};
   double timeOfSavedPosSatE = 0.0;
   bool validSavedPosSatE = false;
-  const float zeroVector[3] = {0.0, 0.0, 0.0};
 
   SusConverter susConverter;
   AcsParameters acsParameters;