diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 7d8f5bd2..ea52f5a2 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -67,16 +67,17 @@ void SusConverter::calcAngle(lp_vec_t 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 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::normalize(sunVectorEIVE, sunVectorEIVE, 3); - } else { - // No sus is valid - throw std::invalid_argument("No sun sensor is valid"); // throw error - } - - return sunVectorEIVE; -}