mission/controller/acs/SusConverter.cpp

@@ -67,16 +67,17 @@ void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> susChannel) {
 }
 
 void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
-  uint8_t index, k, l;
+  uint8_t index;
+  float k, l;
 
   // while loop iterates above all calibration cells to use the different calibration functions in
   // each cell
   k = 0;
   while (k < 3) {
-    k = k + 1;
+    k++;
     l = 0;
     while (l < 3) {
-      l = l + 1;
+      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)) &&
@@ -131,113 +132,3 @@ float* SusConverter::getSunVectorSensorFrame(lp_vec_t<uint16_t, 6> susChannel,
   return calculateSunVector();
 }
 
-bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; }
-
-float* SusConverter::TransferSunVector() {
-  float* sunVectorEIVE = 0;
-  sunVectorEIVE = new float[3];
-
-  uint8_t susAvail = 12;
-  int8_t basisMatrixUse[3][3];
-  float sunVectorMatrixEIVE[3][12] = {0};
-  float sunVectorMatrixBodyFrame[3][12];
-
-  for (uint8_t susNumber = 0; susNumber < 12;
-       susNumber++) {  // save the sun vector of each SUS in their body frame into an array for
-                       // further processing
-    float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber];
-    sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0];
-    sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1];
-    sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2];
-  }
-
-  for (uint8_t susNumber = 0; susNumber < 12; susNumber++) {
-    if (getValidFlag(susNumber) == returnvalue::FAILED) {
-      susAvail -= 1;
-    }  // if the SUS data is not valid ->
-
-    for (uint8_t c1 = 0; c1 < 3; c1++) {
-      for (uint8_t c2 = 0; c2 < 3; c2++) {
-        switch (susNumber) {
-          case 0:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2];
-            break;
-          case 1:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
-            break;
-          case 2:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
-            break;
-          case 3:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
-            break;
-          case 4:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
-            break;
-          case 5:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
-            break;
-          case 6:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
-            break;
-          case 7:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
-            break;
-          case 8:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
-            break;
-          case 9:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
-            break;
-          case 10:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
-            break;
-          case 11:
-            basisMatrixUse[c1][c2] =
-                acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
-            break;
-        }
-      }
-    }
-
-    // matrix multiplication for transition in EIVE coordinatesystem
-    for (uint8_t p = 0; p < 3; p++) {
-      for (uint8_t q = 0; q < 3; q++) {
-        // normal matrix multiplication
-        sunVectorMatrixEIVE[p][susNumber] +=
-            (basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]);
-      }
-    }
-  }
-
-  if (susAvail > 0) {  // Calculate one sun vector out of all sun vectors from the different SUS
-    for (uint8_t i = 0; i < 3; i++) {
-      float sum = 0;
-      for (uint8_t susNumber = 0; susNumber < 12; susNumber++) {
-        if (getValidFlag(susNumber) == returnvalue::OK) {
-          sum += sunVectorMatrixEIVE[i][susNumber];
-          // printf("%f\n", SunVectorMatrixEIVE[i][susNumber]);
-        }
-      }
-      // ToDo: decide on length on sun vector
-      sunVectorEIVE[i] = sum;
-    }
-    VectorOperations<float>::normalize(sunVectorEIVE, sunVectorEIVE, 3);
-  } else {
-    // No sus is valid
-    throw std::invalid_argument("No sun sensor is valid");  // throw error
-  }
-
-  return sunVectorEIVE;
-}