fixed calculation of sun vector

mission/controller/acs/SensorProcessing.cpp

@@ -210,45 +210,26 @@ void SensorProcessing::processSus(
   sunIjkModel[0] = cos(eclipticLongitude);
   sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
   sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
-  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 (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
+  uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-      !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
+  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);
+
+  bool susValid[12] = {true, true, true, true, true, true, true, true, true, true, true, true};
+  bool allInvalid =
+      susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
+
+  if (allInvalid) {
     {
       PoolReadGuard pg(susDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
@@ -274,118 +255,78 @@ void SensorProcessing::processSus(
     }
     return;
   }
-  // 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},
-        sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
-        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 (sus0valid) {
+  float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
-    MatrixOperations<float>::multiply(
+                               {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-        susParameters->sus0orientationMatrix[0],
+  float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
-        susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
+                             {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-                                             susParameters->sus0coeffBeta),
-        sus0VecBody, 3, 3, 1);
-  }
-  if (sus1valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus1orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
-                                             susParameters->sus1coeffBeta),
-        sus1VecBody, 3, 3, 1);
-  }
-  if (sus2valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus2orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
-                                             susParameters->sus2coeffBeta),
-        sus2VecBody, 3, 3, 1);
-  }
-  if (sus3valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus3orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
-                                             susParameters->sus3coeffBeta),
-        sus3VecBody, 3, 3, 1);
-  }
-  if (sus4valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus4orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
-                                             susParameters->sus4coeffBeta),
-        sus4VecBody, 3, 3, 1);
-  }
-  if (sus5valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus5orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
-                                             susParameters->sus5coeffBeta),
-        sus5VecBody, 3, 3, 1);
-  }
-  if (sus6valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus6orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
-                                             susParameters->sus6coeffBeta),
-        sus6VecBody, 3, 3, 1);
-  }
-  if (sus7valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus7orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
-                                             susParameters->sus7coeffBeta),
-        sus7VecBody, 3, 3, 1);
-  }
-  if (sus8valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus8orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
-                                             susParameters->sus8coeffBeta),
-        sus8VecBody, 3, 3, 1);
-  }
-  if (sus9valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus9orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
-                                             susParameters->sus9coeffBeta),
-        sus9VecBody, 3, 3, 1);
-  }
-  if (sus10valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus10orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
-                                             susParameters->sus10coeffBeta),
-        sus10VecBody, 3, 3, 1);
-  }
-  if (sus11valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus11orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
-                                             susParameters->sus11coeffBeta),
-        sus11VecBody, 3, 3, 1);
-  }
 
-  /* ------ Mean Value: susDirEst ------ */
+  if (susValid[0]) {
-  bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
+    susConverter.calculateSunVector(susVecSensor[0], sus0Value);
-                       sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
+    MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
-  float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
+                                      susVecBody[0], 3, 3, 1);
-                              sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
+  }
-                              sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
+  if (susValid[1]) {
-                             {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
+    susConverter.calculateSunVector(susVecSensor[1], sus1Value);
-                              sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
+    MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
-                              sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
+                                      susVecBody[1], 3, 3, 1);
-                             {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
+  }
-                              sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
+  if (susValid[2]) {
-                              sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
+    susConverter.calculateSunVector(susVecSensor[2], sus2Value);
+    MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2],
+                                      susVecBody[2], 3, 3, 1);
+  }
+  if (susValid[3]) {
+    susConverter.calculateSunVector(susVecSensor[3], sus3Value);
+    MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3],
+                                      susVecBody[3], 3, 3, 1);
+  }
+  if (susValid[4]) {
+    susConverter.calculateSunVector(susVecSensor[4], sus4Value);
+    MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4],
+                                      susVecBody[4], 3, 3, 1);
+  }
+  if (susValid[5]) {
+    susConverter.calculateSunVector(susVecSensor[5], sus5Value);
+    MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5],
+                                      susVecBody[5], 3, 3, 1);
+  }
+  if (susValid[6]) {
+    susConverter.calculateSunVector(susVecSensor[6], sus6Value);
+    MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6],
+                                      susVecBody[6], 3, 3, 1);
+  }
+  if (susValid[7]) {
+    susConverter.calculateSunVector(susVecSensor[7], sus7Value);
+    MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7],
+                                      susVecBody[7], 3, 3, 1);
+  }
+  if (susValid[8]) {
+    susConverter.calculateSunVector(susVecSensor[8], sus8Value);
+    MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8],
+                                      susVecBody[8], 3, 3, 1);
+  }
+  if (susValid[9]) {
+    susConverter.calculateSunVector(susVecSensor[9], sus9Value);
+    MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9],
+                                      susVecBody[9], 3, 3, 1);
+  }
+  if (susValid[10]) {
+    susConverter.calculateSunVector(susVecSensor[10], sus10Value);
+    MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10],
+                                      susVecBody[10], 3, 3, 1);
+  }
+  if (susValid[11]) {
+    susConverter.calculateSunVector(susVecSensor[11], sus11Value);
+    MatrixOperations<float>::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11],
+                                      susVecBody[11], 3, 3, 1);
+  }
 
   double susMeanValue[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 12; i++) {
-    if (validIds[i]) {
+    susMeanValue[0] += susVecBody[i][0];
-      susMeanValue[0] += susVecBody[0][i];
+    susMeanValue[1] += susVecBody[i][1];
-      susMeanValue[1] += susVecBody[1][i];
+    susMeanValue[2] += susVecBody[i][2];
-      susMeanValue[2] += susVecBody[2][i];
-    }
   }
   double susVecTot[3] = {0.0, 0.0, 0.0};
   VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
@@ -406,29 +347,29 @@ void SensorProcessing::processSus(
   {
     PoolReadGuard pg(susDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float));
       susDataProcessed->sus0vec.setValid(sus0valid);
-      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float));
       susDataProcessed->sus1vec.setValid(sus1valid);
-      std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float));
       susDataProcessed->sus2vec.setValid(sus2valid);
-      std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float));
       susDataProcessed->sus3vec.setValid(sus3valid);
-      std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float));
       susDataProcessed->sus4vec.setValid(sus4valid);
-      std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float));
       susDataProcessed->sus5vec.setValid(sus5valid);
-      std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float));
       susDataProcessed->sus6vec.setValid(sus6valid);
-      std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float));
       susDataProcessed->sus7vec.setValid(sus7valid);
-      std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float));
       susDataProcessed->sus8vec.setValid(sus8valid);
-      std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float));
       susDataProcessed->sus9vec.setValid(sus9valid);
-      std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float));
       susDataProcessed->sus10vec.setValid(sus10valid);
-      std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float));
       susDataProcessed->sus11vec.setValid(sus11valid);
       std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
       susDataProcessed->susVecTot.setValid(true);

mission/controller/acs/SusConverter.cpp

@@ -1,39 +1,51 @@
 #include "SusConverter.h"
 
-#include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <math.h>
 
-bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
+uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
   if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
       susChannel[0] > susChannel[GNDREF]) {
-    return false;
+    return 0;
   }
   if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
       susChannel[1] > susChannel[GNDREF]) {
-    return false;
+    return 0;
   };
   if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
       susChannel[2] > susChannel[GNDREF]) {
-    return false;
+    return 0;
   };
   if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
       susChannel[3] > susChannel[GNDREF]) {
-    return false;
+    return 0;
   };
 
-  susChannelValueSum =
+  uint64_t susChannelValueSum =
       4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
-  if ((susChannelValueSum < SUS_CHANNEL_SUM_HIGH) &&
+  if (susChannelValueSum < SUS_ALBEDO_CHECK) {
-      (susChannelValueSum > SUS_CHANNEL_SUM_LOW)) {
+    return 0;
-    return false;
   };
-  return true;
+  return susChannelValueSum;
 }
 
-void SusConverter::calcAngle(const uint16_t susChannel[6]) {
+bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12],
-  float s = 0.03;  // s=[mm] gap between diodes
+                                 const float threshold) {
-  float d = 5;     // d=[mm] edge length of the quadratic aperture
+  uint8_t maxBrightness = 0;
-  float h = 1;     // h=[mm] distance between diodes and aperture
+  VectorOperations<uint64_t>::maxValue(brightness, 12, &maxBrightness);
+  if (brightness[maxBrightness] == 0) {
+    return true;
+  }
+  for (uint8_t idx = 0; idx < 12; idx++) {
+    if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) {
+      susValid[idx] = false;
+      continue;
+    }
+    susValid[idx] = true;
+  }
+  return false;
+}
+
+void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) {
   // Substract measurement values from GNDREF zero current threshold
   float ch0 = susChannel[GNDREF] - susChannel[0];
   float ch1 = susChannel[GNDREF] - susChannel[1];
@@ -41,70 +53,12 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) {
   float ch3 = susChannel[GNDREF] - susChannel[3];
 
   // Calculation of x and y
-  float xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
+  float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
-  float yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
+  float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
 
   // Calculation of the angles
-  alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI);  //[°]
+  sunVectorSensorFrame[0] = -xout;
-  alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI);  //[°]
+  sunVectorSensorFrame[1] = -yout;
-}
+  sunVectorSensorFrame[2] = H;
-
-void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
-  uint8_t index, k, l;
-
-  // while loop iterates above all calibration cells to use the different calibration functions in
-  // each cell
-  k = 0;
-  while (k < 3) {
-    k++;
-    l = 0;
-    while (l < 3) {
-      l++;
-      // if-condition to check in which cell the data point has to be
-      if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) &&
-           alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) &&
-          (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) &&
-           alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) {
-        index = (3 * (k - 1) + l) - 1;  // calculate the index of the datapoint for the right cell
-        alphaBetaCalibrated[0] =
-            coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +
-            coeffAlpha[index][2] * alphaBetaRaw[1] +
-            coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
-        alphaBetaCalibrated[1] =
-            coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] +
-            coeffBeta[index][2] * alphaBetaRaw[1] +
-            coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
-      }
-    }
-  }
-}
-
-float* SusConverter::calculateSunVector() {
-  // Calculate the normalized Sun Vector
-  sunVectorSensorFrame[0] = -tan(alphaBetaCalibrated[0] * (M_PI / 180));
-  sunVectorSensorFrame[1] = -tan(alphaBetaCalibrated[1] * (M_PI / 180));
-  sunVectorSensorFrame[2] = 1;
   VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
-
-  return sunVectorSensorFrame;
-}
-
-float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],
-                                             const float coeffAlpha[9][10],
-                                             const float coeffBeta[9][10]) {
-  calcAngle(susChannel);
-  calibration(coeffAlpha, coeffBeta);
-  return calculateSunVector();
 }

mission/controller/acs/SusConverter.h

@@ -1,8 +1,4 @@
-#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
+#include <fsfw/globalfunctions/math/VectorOperations.h>
-#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
-
-#include <fsfw/datapoollocal/LocalPoolVector.h>
-#include <stdint.h>
 
 #include "AcsParameters.h"
 
@@ -10,24 +6,11 @@ class SusConverter {
  public:
   SusConverter() {}
 
-  bool checkSunSensorData(const uint16_t susChannel[6]);
+  uint64_t checkSunSensorData(const uint16_t susChannel[6]);
-
+  bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold);
-  void calcAngle(const uint16_t susChannel[6]);
+  void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]);
-  void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]);
-  float* calculateSunVector();
-
-  float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10],
-                                 const float coeffBeta[9][10]);
 
  private:
-  float alphaBetaRaw[2];          //[°]
-  float alphaBetaCalibrated[2];   //[°]
-  float sunVectorSensorFrame[3];  //[-]
-
-  bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
-                        returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
-                        returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK};
-
   static const uint8_t GNDREF = 4;
   // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check
   static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096;
@@ -35,16 +18,14 @@ class SusConverter {
   static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0;
   // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the
   // reflection of sunlight from the moon/earth
-  static constexpr uint16_t SUS_CHANNEL_SUM_HIGH = 100;
+  static constexpr uint16_t SUS_ALBEDO_CHECK = 1000;
   // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the  reflection
   // of sunlight from the moon/earth
   static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0;
-  // [°] Width of the calibration cells --> necessary for checking in
+
-  // which cell a data point should be
+  static constexpr float S = 0.03;  // S=[mm] gap between diodes
-  static const uint8_t completeCellWidth = 140, halfCellWidth = 70;
+  static constexpr float D = 5;     // D=[mm] edge length of the quadratic aperture
-  uint16_t susChannelValueSum = 0;
+  static constexpr float H = 1;     // H=[mm] distance between diodes and aperture
 
   AcsParameters acsParameters;
 };
-
-#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */