From e70ee1bf9d61d5a7cffa64bdf24e3e0bbf3278a7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 23 Sep 2022 09:56:32 +0200 Subject: [PATCH] initial commit of SusConverter --- mission/controller/acs/SusConverter.cpp | 382 ++++++++++++++++++++++++ mission/controller/acs/SusConverter.h | 59 ++++ 2 files changed, 441 insertions(+) create mode 100644 mission/controller/acs/SusConverter.cpp create mode 100644 mission/controller/acs/SusConverter.h diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp new file mode 100644 index 00000000..0ef2fd28 --- /dev/null +++ b/mission/controller/acs/SusConverter.cpp @@ -0,0 +1,382 @@ +/* + * SusConverter.cpp + * + * Created on: 17.01.2022 + * Author: Timon Schwarz + */ + +#include //for atan2 +#include +#include + +void SunSensor::setSunSensorData(uint8_t Sensornumber) { + // Creates dummy sensordata, replace with SUS devicehandler / channel readout + ChannelValue[0] = 3913; + ChannelValue[1] = 3912; + ChannelValue[2] = 3799; + ChannelValue[3] = 3797; + ChannelValue[4] = 4056; +} + +void SunSensor::checkSunSensorData(uint8_t Sensornumber) { + uint16_t ChannelValueSum; + + // 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 + // channel measurement range? + ValidityNumber = 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( + "The value of channel %i from sun sensor %i is higher than the zero current threshold " + "GNDREF\n", + k, Sensornumber); + }; + }; + + // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is + // 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]); + if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { + ValidityNumber = false; + printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + }; +} + +void SunSensor::AngleCalculation() { + 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]; + + // 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); //[°] +} + +void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { + switch (Sensornumber) { // search for the correct calibration coefficients for each SUS + + case 0: + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + + 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]; + } + } + break; + } +} + +void SunSensor::Calibration() { + float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; + uint8_t index; + + alpha_m = AlphaBetaRaw[0]; //[°] + beta_m = AlphaBetaRaw[1]; //[°] + + // while loop iterates above all calibration cells to use the different calibration functions in + // each cell + k = 0; + while (k < 3) { + k = k + 1; + l = 0; + while (l < 3) { + l = l + 1; + // if-condition to check in which cell the data point has to be + if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && + alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && + (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && + 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 + 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; + } + } + } + + // while loop iterates above all calibration cells to use the different calibration functions in + // each cell + k = 0; + while (k < 3) { + k = k + 1; + l = 0; + while (l < 3) { + l = l + 1; + // if condition to check in which cell the data point has to be + if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && + alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && + (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && + 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; + } + } + } + + AlphaBetaCalibrated[0] = alpha_calibrated; //[°] + AlphaBetaCalibrated[1] = beta_calibrated; //[°] +} + +void SunSensor::CalculateSunVector() { + float alpha, beta; + alpha = AlphaBetaCalibrated[0]; //[°] + beta = AlphaBetaCalibrated[1]; //[°] + + // Calculate the normalized Sun Vector + SunVectorBodyFrame[0] = + (tan(alpha * (M_PI / 180)) / + (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); + SunVectorBodyFrame[1] = + (tan(beta * (M_PI / 180)) / + (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); + SunVectorBodyFrame[2] = + (-1 / + (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); +} + +float* SunSensor::getSunVectorBodyFrame() { + // return function for the sun vector in the body frame + float* SunVectorBodyFrameReturn = 0; + SunVectorBodyFrameReturn = new float[3]; + + SunVectorBodyFrameReturn[0] = SunVectorBodyFrame[0]; + SunVectorBodyFrameReturn[1] = SunVectorBodyFrame[1]; + SunVectorBodyFrameReturn[2] = SunVectorBodyFrame[2]; + + return SunVectorBodyFrameReturn; +} + +float* SunSensor::TransferSunVector(SunSensor SUS[12]) { + float* SunVectorEIVE = 0; + SunVectorEIVE = new float[3]; + + uint8_t counter = 0; + int8_t BasisMatrixUse[3][3]; + float SunVectorMatrixEIVE[3][12] = {0}, sum; + float SunVectorMatrixBodyFrame[3][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(); + 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) { + counter = counter + 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 (Sensornumber) { // find right basis matrix for each SUS + + case 0: + BasisMatrixUse[c1][c2] = AcsParameters[c1][c2]; + break; + case 1: + BasisMatrixUse[c1][c2] = BasisMatrix1[c1][c2]; + break; + case 2: + BasisMatrixUse[c1][c2] = BasisMatrix2[c1][c2]; + break; + case 3: + BasisMatrixUse[c1][c2] = BasisMatrix3[c1][c2]; + break; + case 4: + BasisMatrixUse[c1][c2] = BasisMatrix4[c1][c2]; + break; + case 5: + BasisMatrixUse[c1][c2] = BasisMatrix5[c1][c2]; + break; + case 6: + BasisMatrixUse[c1][c2] = BasisMatrix6[c1][c2]; + break; + case 7: + BasisMatrixUse[c1][c2] = BasisMatrix7[c1][c2]; + break; + case 8: + BasisMatrixUse[c1][c2] = BasisMatrix8[c1][c2]; + break; + case 9: + BasisMatrixUse[c1][c2] = BasisMatrix9[c1][c2]; + break; + case 10: + BasisMatrixUse[c1][c2] = BasisMatrix10[c1][c2]; + break; + case 11: + BasisMatrixUse[c1][c2] = BasisMatrix11[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][Sensornumber] += + (BasisMatrixUse[p][q] * SunVectorMatrixBodyFrame[q][Sensornumber]); + } + } + } + + // 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]); + } + SunVectorEIVE[i] = + 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 + } + } else { + // No sus is valid + throw std::invalid_argument("No sun sensor is valid"); // throw error + } + + return SunVectorEIVE; +} + + diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h new file mode 100644 index 00000000..6f1b7420 --- /dev/null +++ b/mission/controller/acs/SusConverter.h @@ -0,0 +1,59 @@ +/* + * SusConverter.h + * + * Created on: Sep 22, 2022 + * Author: marius + */ + +#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ +#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ + + +#include +#include + +class SunSensor { + public: + SunSensor() {} + + void setSunSensorData(uint8_t Sensornumber); + void checkSunSensorData(uint8_t Sensornumber); + void AngleCalculation(); + void setCalibrationCoefficients(uint8_t Sensornumber); + void Calibration(); + void CalculateSunVector(); + + bool getValidityNumber() { return ValidityNumber; } + float* getSunVectorBodyFrame(); + float* TransferSunVector(SunSensor SUS[12]); + + private: + uint16_t ChannelValue[5]; //[Bit] + float AlphaBetaRaw[2]; //[°] + float AlphaBetaCalibrated[2]; //[°] + float SunVectorBodyFrame[3]; //[-] + + bool ValidityNumber = true; + + uint16_t ChannelValueCheckHigh = + 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + uint8_t ChannelValueCheckLow = + 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check + uint16_t ChannelValueSumHigh = + 100; // 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 + uint8_t ChannelValueSumLow = + 0; //[Bit]low borderline for check if the sun sensor is illuminated + // by the sun or by the reflection of sunlight from the moon/earth + uint8_t CompleteCellWidth = 140, + 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]; + + AcsParameters acsParameters; +}; + + +#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */