switched sensor input from AcsController to SensorValues

mission/controller/acs/SensorProcessing.cpp

@@ -19,8 +19,6 @@
 #include "util/MathOperations.h"
 
 using namespace Math;
-// Thought: Maybe separate file for transforming of sensor values
-// into geometry frame and body frame
 
 SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} {
   validMagField = false;
@@ -144,29 +142,56 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   return true;
 }
 
-void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement,
-                                  const AcsParameters::SusHandlingParameters *susParameters,
-                                  const AcsParameters::SunModelParameters *sunModelParameters,
-                                  double *sunDirEst, bool *sunDirEstValid,
-                                  double *sunVectorInertial, bool *sunVectorInertialValid,
-                                  double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
-  susData->sus0.setValid(susConverter.checkSunSensorData(susData->sus0));
-  susData->sus1.setValid(susConverter.checkSunSensorData(susData->sus1));
-  susData->sus2.setValid(susConverter.checkSunSensorData(susData->sus2));
-  susData->sus3.setValid(susConverter.checkSunSensorData(susData->sus3));
-  susData->sus4.setValid(susConverter.checkSunSensorData(susData->sus4));
-  susData->sus5.setValid(susConverter.checkSunSensorData(susData->sus5));
-  susData->sus6.setValid(susConverter.checkSunSensorData(susData->sus6));
-  susData->sus7.setValid(susConverter.checkSunSensorData(susData->sus7));
-  susData->sus8.setValid(susConverter.checkSunSensorData(susData->sus8));
-  susData->sus9.setValid(susConverter.checkSunSensorData(susData->sus9));
-  susData->sus10.setValid(susConverter.checkSunSensorData(susData->sus10));
-  susData->sus11.setValid(susConverter.checkSunSensorData(susData->sus11));
+void SensorProcessing::processSus(
+    const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid,
+    const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid,
+    const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid,
+    const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
+    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,
+    bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
+    double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
+  if (sus0valid) {
+    sus0valid = susConverter.checkSunSensorData(sus0Value);
+  }
+  if (sus1valid) {
+    sus1valid = susConverter.checkSunSensorData(sus1Value);
+  }
+  if (sus2valid) {
+    sus2valid = susConverter.checkSunSensorData(sus2Value);
+  }
+  if (sus3valid) {
+    sus3valid = susConverter.checkSunSensorData(sus3Value);
+  }
+  if (sus4valid) {
+    sus4valid = susConverter.checkSunSensorData(sus4Value);
+  }
+  if (sus5valid) {
+    sus5valid = susConverter.checkSunSensorData(sus5Value);
+  }
+  if (sus6valid) {
+    sus6valid = susConverter.checkSunSensorData(sus6Value);
+  }
+  if (sus7valid) {
+    sus7valid = susConverter.checkSunSensorData(sus7Value);
+  }
+  if (sus8valid) {
+    sus8valid = susConverter.checkSunSensorData(sus8Value);
+  }
+  if (sus9valid) {
+    sus9valid = susConverter.checkSunSensorData(sus9Value);
+  }
+  if (sus10valid) {
+    sus10valid = susConverter.checkSunSensorData(sus10Value);
+  }
+  if (sus11valid) {
+    sus11valid = susConverter.checkSunSensorData(sus11Value);
+  }
 
-  if (!susData->sus0.isValid() && !susData->sus1.isValid() && !susData->sus2.isValid() &&
-      !susData->sus3.isValid() && !susData->sus4.isValid() && !susData->sus5.isValid() &&
-      !susData->sus6.isValid() && !susData->sus7.isValid() && !susData->sus8.isValid() &&
-      !susData->sus9.isValid() && !susData->sus10.isValid() && !susData->sus11.isValid()) {
+  if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
+      !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
     *sunDirEstValid = false;
     return;
   } else {
@@ -177,97 +202,94 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu
           sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
           sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
 
-    if (susData->sus0.isValid()) {
+    if (sus0valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus0orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
                                                susParameters->sus0coeffBeta),
           sus0VecBody, 3, 3, 1);
     }
-    if (susData->sus1.isValid()) {
+    if (sus1valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus1orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
                                                susParameters->sus1coeffBeta),
           sus1VecBody, 3, 3, 1);
     }
-    if (susData->sus2.isValid()) {
+    if (sus2valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus2orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
                                                susParameters->sus2coeffBeta),
           sus2VecBody, 3, 3, 1);
     }
-    if (susData->sus3.isValid()) {
+    if (sus3valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus3orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
                                                susParameters->sus3coeffBeta),
           sus3VecBody, 3, 3, 1);
     }
-    if (susData->sus4.isValid()) {
+    if (sus4valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus4orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
                                                susParameters->sus4coeffBeta),
           sus4VecBody, 3, 3, 1);
     }
-    if (susData->sus5.isValid()) {
+    if (sus5valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus5orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
                                                susParameters->sus5coeffBeta),
           sus5VecBody, 3, 3, 1);
     }
-    if (susData->sus6.isValid()) {
+    if (sus6valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus6orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
                                                susParameters->sus6coeffBeta),
           sus6VecBody, 3, 3, 1);
     }
-    if (susData->sus7.isValid()) {
+    if (sus7valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus7orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
                                                susParameters->sus7coeffBeta),
           sus7VecBody, 3, 3, 1);
     }
-    if (susData->sus8.isValid()) {
+    if (sus8valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus8orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
                                                susParameters->sus8coeffBeta),
           sus8VecBody, 3, 3, 1);
     }
-    if (susData->sus9.isValid()) {
+    if (sus9valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus9orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
                                                susParameters->sus9coeffBeta),
           sus9VecBody, 3, 3, 1);
     }
-    if (susData->sus10.isValid()) {
+    if (sus10valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus10orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
                                                susParameters->sus10coeffBeta),
           sus10VecBody, 3, 3, 1);
     }
-    if (susData->sus11.isValid()) {
+    if (sus11valid) {
       MatrixOperations<float>::multiply(
           susParameters->sus11orientationMatrix[0],
-          susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha,
+          susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
                                                susParameters->sus11coeffBeta),
           sus11VecBody, 3, 3, 1);
     }
 
     /* ------ Mean Value: susDirEst ------ */
-    bool validIds[12] = {
-        susData->sus0.isValid(), susData->sus1.isValid(),  susData->sus2.isValid(),
-        susData->sus3.isValid(), susData->sus4.isValid(),  susData->sus5.isValid(),
-        susData->sus6.isValid(), susData->sus7.isValid(),  susData->sus8.isValid(),
-        susData->sus9.isValid(), susData->sus10.isValid(), susData->sus11.isValid()};
+    bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
+                         sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
     float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
                                 sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
                                 sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
@@ -279,17 +301,14 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu
                                 sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
 
     double susMeanValue[3] = {0, 0, 0};
-    float validSusCounter = 0;
     for (uint8_t i = 0; i < 12; i++) {
       if (validIds[i]) {
         susMeanValue[0] += susVecBody[0][i];
         susMeanValue[1] += susVecBody[1][i];
         susMeanValue[2] += susVecBody[2][i];
-        validSusCounter += 1;
       }
     }
-    double divisor = 1 / validSusCounter;
-    VectorOperations<double>::mulScalar(susMeanValue, divisor, sunDirEst, 3);
+    VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
     *sunDirEstValid = true;
   }
 
@@ -410,26 +429,40 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
 void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now,
                                ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
                                const AcsParameters *acsParameters) {
-  //  sensorValues->update();
+  sensorValues->update();
   //  processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid,
   //             &outputValues->gcLatitude, &outputValues->gdLongitude);
 
-  //  outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid,
-  //                  sensorValues->mgm1, sensorValues->mgm1Valid,
-  //                  sensorValues->mgm2, sensorValues->mgm2Valid,
-  //                  sensorValues->mgm3, sensorValues->mgm3Valid,
-  //                  sensorValues->mgm4, sensorValues->mgm4Valid, now,
-  //                  &acsParameters->mgmHandlingParameters, outputValues->gcLatitude,
-  //                  outputValues->gdLongitude, sensorValues->gps0altitude,
-  //                  sensorValues->gps0Valid,
-  //                  outputValues->magFieldEst, &outputValues->magFieldEstValid,
-  //                  outputValues->magFieldModel, &outputValues->magFieldModelValid,
-  //                  outputValues->magneticFieldVectorDerivative,
-  //  &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?
+  outputValues->mgmUpdated = processMgm(
+      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->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
+      outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude,
+      sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid,
+      outputValues->magFieldModel, &outputValues->magFieldModelValid,
+      outputValues->magneticFieldVectorDerivative,
+      &outputValues->magneticFieldVectorDerivativeValid);  // VALID outputs- PoolVariable ?
 
-  processSus(susData, now, &acsParameters->susHandlingParameters,
-             &acsParameters->sunModelParameters, outputValues->sunDirEst,
-             &outputValues->sunDirEstValid, outputValues->sunDirModel,
+  processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
+             sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
+             sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(),
+             sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(),
+             sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(),
+             sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(),
+             sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(),
+             sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(),
+             sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(),
+             sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
+             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,
              &outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
              &outputValues->sunVectorDerivativeValid);
   // VALID outputs ?