From 46ec376e4d40ed4ad9c7c67ce150185ba5112859 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 23 Sep 2022 14:27:05 +0200 Subject: [PATCH] amended SusConverter for use as OBSW --- mission/controller/acs/SusConverter.cpp | 208 +++++++++++++----------- mission/controller/acs/SusConverter.h | 26 +-- 2 files changed, 125 insertions(+), 109 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 0ef2fd28..d29f3706 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -8,14 +8,22 @@ #include //for atan2 #include #include +#include -void SunSensor::setSunSensorData(uint8_t Sensornumber) { +void SunSensor::setSunSensorData() { // Creates dummy sensordata, replace with SUS devicehandler / channel readout - ChannelValue[0] = 3913; - ChannelValue[1] = 3912; - ChannelValue[2] = 3799; - ChannelValue[3] = 3797; - ChannelValue[4] = 4056; + susChannelValues[0] = {3913, 3912, 3799, 4056}; + susChannelValues[1] = {3913, 3912, 3799, 4056}; + susChannelValues[2] = {3913, 3912, 3799, 4056}; + susChannelValues[3] = {3913, 3912, 3799, 4056}; + susChannelValues[4] = {3913, 3912, 3799, 4056}; + susChannelValues[5] = {3913, 3912, 3799, 4056}; + susChannelValues[6] = {3913, 3912, 3799, 4056}; + susChannelValues[7] = {3913, 3912, 3799, 4056}; + susChannelValues[8] = {3913, 3912, 3799, 4056}; + susChannelValues[9] = {3913, 3912, 3799, 4056}; + susChannelValues[10] = {3913, 3912, 3799, 4056}; + susChannelValues[11] = {3913, 3912, 3799, 4056}; } void SunSensor::checkSunSensorData(uint8_t Sensornumber) { @@ -24,21 +32,21 @@ void SunSensor::checkSunSensorData(uint8_t Sensornumber) { // Check individual channel values for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - if (ChannelValue[k] <= ChannelValueCheckLow || - ChannelValue[k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS + if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow || + susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS // channel measurement range? - ValidityNumber = false; // false --> Data not valid - printf( + ValidityNumber[Sensornumber] = false; // false --> Data not valid + /*printf( "The value of channel %i from sun sensor %i is not inside the borders of valid data with " "a value of %i \n", - k, Sensornumber, ChannelValue[k]); - } else if (ChannelValue[k] > - ChannelValue[4]) { // Channel values higher than zero current threshold GNDREF? - ValidityNumber = false; - printf( + k, Sensornumber, ChannelValue[k]);*/ + } else if (susChannelValues[Sensornumber][k] > + susChannelValues[Sensornumber][4]) { // Channel values higher than zero current threshold GNDREF? + ValidityNumber[Sensornumber] = false; + /*printf( "The value of channel %i from sun sensor %i is higher than the zero current threshold " "GNDREF\n", - k, Sensornumber); + k, Sensornumber);*/ }; }; @@ -46,30 +54,32 @@ void SunSensor::checkSunSensorData(uint8_t Sensornumber) { // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon // reflection or earth albedo) ChannelValueSum = - 4 * ChannelValue[4] - (ChannelValue[0] + ChannelValue[1] + ChannelValue[2] + ChannelValue[3]); + 4 * susChannelValues[Sensornumber][4] - (susChannelValues[Sensornumber][0] + + susChannelValues[Sensornumber][1] + susChannelValues[Sensornumber][2] + + susChannelValues[Sensornumber][3]); if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { - ValidityNumber = false; - printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + ValidityNumber[Sensornumber] = false; + //printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); }; } -void SunSensor::AngleCalculation() { +void SunSensor::AngleCalculation(uint8_t Sensornumber) { float xout, yout, s = 0.03; // s=[mm] uint8_t d = 5, h = 1; // d=[mm] h=[mm] int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = ChannelValue[4] - ChannelValue[0]; - ch1 = ChannelValue[4] - ChannelValue[1]; - ch2 = ChannelValue[4] - ChannelValue[2]; - ch3 = ChannelValue[4] - ChannelValue[3]; + ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0]; + ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1]; + ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2]; + ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] 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); //[°] - AlphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] + AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI); //[°] + AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI); //[°] } void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { @@ -79,8 +89,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { for (uint8_t row = 0; row < 9; row++) { // save the correct coefficients in the right SUS class for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; } } break; @@ -88,8 +98,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 1: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; } } break; @@ -97,8 +107,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 2: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; } } break; @@ -106,8 +116,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 3: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; } } break; @@ -115,8 +125,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 4: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; } } break; @@ -124,8 +134,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 5: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; } } break; @@ -133,8 +143,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 6: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; } } break; @@ -142,8 +152,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 7: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; } } break; @@ -151,8 +161,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 8: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; } } break; @@ -160,8 +170,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 9: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; } } break; @@ -169,8 +179,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 10: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; } } break; @@ -178,20 +188,20 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 11: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; } } break; } } -void SunSensor::Calibration() { +void SunSensor::Calibration(uint8_t Sensornumber) { float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; uint8_t index; - alpha_m = AlphaBetaRaw[0]; //[°] - beta_m = AlphaBetaRaw[1]; //[°] + alpha_m = AlphaBetaRaw[Sensornumber][0]; //[°] + beta_m = AlphaBetaRaw[Sensornumber][1]; //[°] // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -209,13 +219,13 @@ void SunSensor::Calibration() { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 alpha_calibrated = - CoeffAlpha[index][0] + CoeffAlpha[index][1] * alpha_m + CoeffAlpha[index][2] * beta_m + - CoeffAlpha[index][3] * alpha_m * alpha_m + CoeffAlpha[index][4] * alpha_m * beta_m + - CoeffAlpha[index][5] * beta_m * beta_m + - CoeffAlpha[index][6] * alpha_m * alpha_m * alpha_m + - CoeffAlpha[index][7] * alpha_m * alpha_m * beta_m + - CoeffAlpha[index][8] * alpha_m * beta_m * beta_m + - CoeffAlpha[index][9] * beta_m * beta_m * beta_m; + CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m + + CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m + + CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m + + CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + + CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + + CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m + + CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; } } } @@ -235,35 +245,35 @@ void SunSensor::Calibration() { beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 - beta_calibrated = CoeffBeta[index][0] + CoeffBeta[index][1] * alpha_m + - CoeffBeta[index][2] * beta_m + CoeffBeta[index][3] * alpha_m * alpha_m + - CoeffBeta[index][4] * alpha_m * beta_m + - CoeffBeta[index][5] * beta_m * beta_m + - CoeffBeta[index][6] * alpha_m * alpha_m * alpha_m + - CoeffBeta[index][7] * alpha_m * alpha_m * beta_m + - CoeffBeta[index][8] * alpha_m * beta_m * beta_m + - CoeffBeta[index][9] * beta_m * beta_m * beta_m; + beta_calibrated = CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + + CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + + CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + + CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; } } } - AlphaBetaCalibrated[0] = alpha_calibrated; //[°] - AlphaBetaCalibrated[1] = beta_calibrated; //[°] + AlphaBetaCalibrated[Sensornumber][0] = alpha_calibrated; //[°] + AlphaBetaCalibrated[Sensornumber][1] = beta_calibrated; //[°] } -void SunSensor::CalculateSunVector() { +void SunSensor::CalculateSunVector(uint8_t Sensornumber) { float alpha, beta; - alpha = AlphaBetaCalibrated[0]; //[°] - beta = AlphaBetaCalibrated[1]; //[°] + alpha = AlphaBetaCalibrated[Sensornumber][0]; //[°] + beta = AlphaBetaCalibrated[Sensornumber][1]; //[°] // Calculate the normalized Sun Vector - SunVectorBodyFrame[0] = + SunVectorBodyFrame[Sensornumber][0] = (tan(alpha * (M_PI / 180)) / (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[1] = + SunVectorBodyFrame[Sensornumber][1] = (tan(beta * (M_PI / 180)) / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[2] = + SunVectorBodyFrame[Sensornumber][2] = (-1 / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } @@ -280,7 +290,11 @@ float* SunSensor::getSunVectorBodyFrame() { return SunVectorBodyFrameReturn; } -float* SunSensor::TransferSunVector(SunSensor SUS[12]) { +bool SunSensor::getValidityNumber(uint8_t Sensornumber) { + return ValidityNumber[Sensornumber]; +} + +float* SunSensor::TransferSunVector() { float* SunVectorEIVE = 0; SunVectorEIVE = new float[3]; @@ -292,14 +306,14 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for // further processing - float* SunVectorBodyFrame = this[Sensornumber].getSunVectorBodyFrame(); + float* SunVectorBodyFrame = SunVectorBodyFrame[Sensornumber]; SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; } for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (SUS[Sensornumber].getValidityNumber() == false) { + if (getValidityNumber(Sensornumber) == false) { counter = counter + 1; } // if the SUS data is not valid -> @@ -308,40 +322,40 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { switch (Sensornumber) { // find right basis matrix for each SUS case 0: - BasisMatrixUse[c1][c2] = AcsParameters[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - BasisMatrixUse[c1][c2] = BasisMatrix1[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - BasisMatrixUse[c1][c2] = BasisMatrix2[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - BasisMatrixUse[c1][c2] = BasisMatrix3[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - BasisMatrixUse[c1][c2] = BasisMatrix4[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - BasisMatrixUse[c1][c2] = BasisMatrix5[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - BasisMatrixUse[c1][c2] = BasisMatrix6[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - BasisMatrixUse[c1][c2] = BasisMatrix7[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - BasisMatrixUse[c1][c2] = BasisMatrix8[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - BasisMatrixUse[c1][c2] = BasisMatrix9[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - BasisMatrixUse[c1][c2] = BasisMatrix10[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - BasisMatrixUse[c1][c2] = BasisMatrix11[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -357,20 +371,22 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { } } - // ToDo: remove invalid SUSs from being used for calculating the combined sun vector - if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS for (uint8_t i = 0; i < 3; i++) { sum = 0; for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - sum += SunVectorMatrixEIVE[i][Sensornumber]; - printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + if (getValidityNumber(Sensornumber)){ + sum += SunVectorMatrixEIVE[i][Sensornumber]; + //printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + } } + // ToDo: decide on length on sun vector SunVectorEIVE[i] = - sum / (12 - counter); // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm + sum/* / (12 - counter)*/; // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm // liegen, vermutlich aber an den falschen ChannelValues da die // transformierten Sonnenvektoren jedes SUS plausibel sind } + VectorOperations::normalize(SunVectorEIVE, SunVectorEIVE, 3); } else { // No sus is valid throw std::invalid_argument("No sun sensor is valid"); // throw error diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 6f1b7420..4012bb5a 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -16,24 +16,24 @@ class SunSensor { public: SunSensor() {} - void setSunSensorData(uint8_t Sensornumber); + void setSunSensorData(); void checkSunSensorData(uint8_t Sensornumber); - void AngleCalculation(); + void AngleCalculation(uint8_t Sensornumber); void setCalibrationCoefficients(uint8_t Sensornumber); - void Calibration(); - void CalculateSunVector(); + void Calibration(uint8_t Sensornumber); + void CalculateSunVector(uint8_t Sensornumber); - bool getValidityNumber() { return ValidityNumber; } + bool getValidityNumber(uint8_t Sensornumber); float* getSunVectorBodyFrame(); - float* TransferSunVector(SunSensor SUS[12]); + float* TransferSunVector(); private: - uint16_t ChannelValue[5]; //[Bit] - float AlphaBetaRaw[2]; //[°] - float AlphaBetaCalibrated[2]; //[°] - float SunVectorBodyFrame[3]; //[-] + uint16_t susChannelValues[12][4]; //[Bit] + float AlphaBetaRaw[12][2]; //[°] + float AlphaBetaCalibrated[12][2]; //[°] + float SunVectorBodyFrame[12][3]; //[-] - bool ValidityNumber = true; + bool ValidityNumber[12] = true; uint16_t ChannelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check @@ -49,8 +49,8 @@ class SunSensor { HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in // which cell a data point should be - float CoeffAlpha[9][10]; - float CoeffBeta[9][10]; + float CoeffAlpha[12][9][10]; + float CoeffBeta[12][9][10]; AcsParameters acsParameters; };