eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

amended SusConverter for use as OBSW
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-09-23 14:27:05 +02:00
parent e70ee1bf9d
commit 46ec376e4d
2 changed files with 125 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

208
mission/controller/acs/SusConverter.cpp
View File

@ -8,14 +8,22 @@
#include <math.h> //for atan2 #include <math.h> //for atan2
#include <iostream> #include <iostream>
#include <SusConverter.h> #include <SusConverter.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
void SunSensor::setSunSensorData(uint8_t Sensornumber) { void SunSensor::setSunSensorData() {
// Creates dummy sensordata, replace with SUS devicehandler / channel readout // Creates dummy sensordata, replace with SUS devicehandler / channel readout
ChannelValue[0] = 3913; susChannelValues[0] = {3913, 3912, 3799, 4056};
ChannelValue[1] = 3912; susChannelValues[1] = {3913, 3912, 3799, 4056};
ChannelValue[2] = 3799; susChannelValues[2] = {3913, 3912, 3799, 4056};
ChannelValue[3] = 3797; susChannelValues[3] = {3913, 3912, 3799, 4056};
ChannelValue[4] = 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) { void SunSensor::checkSunSensorData(uint8_t Sensornumber) {
@ -24,21 +32,21 @@ void SunSensor::checkSunSensorData(uint8_t Sensornumber) {
// Check individual channel values // Check individual channel values
for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters
if (ChannelValue[k] <= ChannelValueCheckLow || if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow ||
ChannelValue[k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS
// channel measurement range? // channel measurement range?
ValidityNumber = false; // false --> Data not valid ValidityNumber[Sensornumber] = false; // false --> Data not valid
printf( /*printf(
"The value of channel %i from sun sensor %i is not inside the borders of valid data with " "The value of channel %i from sun sensor %i is not inside the borders of valid data with "
"a value of %i \n", "a value of %i \n",
k, Sensornumber, ChannelValue[k]); k, Sensornumber, ChannelValue[k]);*/
} else if (ChannelValue[k] > } else if (susChannelValues[Sensornumber][k] >
ChannelValue[4]) { // Channel values higher than zero current threshold GNDREF? susChannelValues[Sensornumber][4]) { // Channel values higher than zero current threshold GNDREF?
ValidityNumber = false; ValidityNumber[Sensornumber] = false;
printf( /*printf(
"The value of channel %i from sun sensor %i is higher than the zero current threshold " "The value of channel %i from sun sensor %i is higher than the zero current threshold "
"GNDREF\n", "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 // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon
// reflection or earth albedo) // reflection or earth albedo)
ChannelValueSum = 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)) { if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) {
ValidityNumber = false; ValidityNumber[Sensornumber] = false;
printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); //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] float xout, yout, s = 0.03; // s=[mm]
uint8_t d = 5, h = 1; // d=[mm] h=[mm] uint8_t d = 5, h = 1; // d=[mm] h=[mm]
int ch0, ch1, ch2, ch3; int ch0, ch1, ch2, ch3;
// Substract measurement values from GNDREF zero current threshold // Substract measurement values from GNDREF zero current threshold
ch0 = ChannelValue[4] - ChannelValue[0]; ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0];
ch1 = ChannelValue[4] - ChannelValue[1]; ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1];
ch2 = ChannelValue[4] - ChannelValue[2]; ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2];
ch3 = ChannelValue[4] - ChannelValue[3]; ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3];
// Calculation of x and y // Calculation of x and y
xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] 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] yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
// Calculation of the angles // Calculation of the angles
AlphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI); //[°]
AlphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI); //[°]
} }
void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
@ -79,8 +89,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
for (uint8_t row = 0; row < 9; for (uint8_t row = 0; row < 9;
row++) { // save the correct coefficients in the right SUS class row++) { // save the correct coefficients in the right SUS class
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column];
} }
} }
break; break;
@ -88,8 +98,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 1: case 1:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column];
} }
} }
break; break;
@ -97,8 +107,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 2: case 2:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column];
} }
} }
break; break;
@ -106,8 +116,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 3: case 3:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column];
} }
} }
break; break;
@ -115,8 +125,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 4: case 4:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column];
} }
} }
break; break;
@ -124,8 +134,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 5: case 5:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column];
} }
} }
break; break;
@ -133,8 +143,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 6: case 6:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column];
} }
} }
break; break;
@ -142,8 +152,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 7: case 7:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column];
} }
} }
break; break;
@ -151,8 +161,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 8: case 8:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column];
} }
} }
break; break;
@ -160,8 +170,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 9: case 9:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column];
} }
} }
break; break;
@ -169,8 +179,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 10: case 10:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column];
} }
} }
break; break;
@ -178,20 +188,20 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) {
case 11: case 11:
for (uint8_t row = 0; row < 9; row++) { for (uint8_t row = 0; row < 9; row++) {
for (uint8_t column = 0; column < 10; column++) { for (uint8_t column = 0; column < 10; column++) {
CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column];
CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column];
} }
} }
break; break;
} }
} }
void SunSensor::Calibration() { void SunSensor::Calibration(uint8_t Sensornumber) {
float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l;
uint8_t index; uint8_t index;
alpha_m = AlphaBetaRaw[0]; //[°] alpha_m = AlphaBetaRaw[Sensornumber][0]; //[°]
beta_m = AlphaBetaRaw[1]; //[°] beta_m = AlphaBetaRaw[Sensornumber][1]; //[°]
// while loop iterates above all calibration cells to use the different calibration functions in // while loop iterates above all calibration cells to use the different calibration functions in
// each cell // 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 index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell
// -> first cell has number 0 // -> first cell has number 0
alpha_calibrated = alpha_calibrated =
CoeffAlpha[index][0] + CoeffAlpha[index][1] * alpha_m + CoeffAlpha[index][2] * beta_m + CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m +
CoeffAlpha[index][3] * alpha_m * alpha_m + CoeffAlpha[index][4] * alpha_m * beta_m + CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m +
CoeffAlpha[index][5] * beta_m * beta_m + CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m +
CoeffAlpha[index][6] * alpha_m * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m +
CoeffAlpha[index][7] * alpha_m * alpha_m * beta_m + CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m +
CoeffAlpha[index][8] * alpha_m * beta_m * beta_m + CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m +
CoeffAlpha[index][9] * beta_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))) { beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) {
index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell
// -> first cell has number 0 // -> first cell has number 0
beta_calibrated = CoeffBeta[index][0] + CoeffBeta[index][1] * alpha_m + beta_calibrated = CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m +
CoeffBeta[index][2] * beta_m + CoeffBeta[index][3] * alpha_m * alpha_m + CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m +
CoeffBeta[index][4] * alpha_m * beta_m + CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m +
CoeffBeta[index][5] * beta_m * beta_m + CoeffBeta[Sensornumber][index][5] * beta_m * beta_m +
CoeffBeta[index][6] * alpha_m * alpha_m * alpha_m + CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m +
CoeffBeta[index][7] * alpha_m * alpha_m * beta_m + CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m +
CoeffBeta[index][8] * alpha_m * beta_m * beta_m + CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m +
CoeffBeta[index][9] * beta_m * beta_m * beta_m; CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m;
} }
} }
} }
AlphaBetaCalibrated[0] = alpha_calibrated; //[°] AlphaBetaCalibrated[Sensornumber][0] = alpha_calibrated; //[°]
AlphaBetaCalibrated[1] = beta_calibrated; //[°] AlphaBetaCalibrated[Sensornumber][1] = beta_calibrated; //[°]
} }
void SunSensor::CalculateSunVector() { void SunSensor::CalculateSunVector(uint8_t Sensornumber) {
float alpha, beta; float alpha, beta;
alpha = AlphaBetaCalibrated[0]; //[°] alpha = AlphaBetaCalibrated[Sensornumber][0]; //[°]
beta = AlphaBetaCalibrated[1]; //[°] beta = AlphaBetaCalibrated[Sensornumber][1]; //[°]
// Calculate the normalized Sun Vector // Calculate the normalized Sun Vector
SunVectorBodyFrame[0] = SunVectorBodyFrame[Sensornumber][0] =
(tan(alpha * (M_PI / 180)) / (tan(alpha * (M_PI / 180)) /
(sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); (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)) / (tan(beta * (M_PI / 180)) /
(sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1))));
SunVectorBodyFrame[2] = SunVectorBodyFrame[Sensornumber][2] =
(-1 / (-1 /
(sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 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; return SunVectorBodyFrameReturn;
} }
float* SunSensor::TransferSunVector(SunSensor SUS[12]) { bool SunSensor::getValidityNumber(uint8_t Sensornumber) {
return ValidityNumber[Sensornumber];
}
float* SunSensor::TransferSunVector() {
float* SunVectorEIVE = 0; float* SunVectorEIVE = 0;
SunVectorEIVE = new float[3]; SunVectorEIVE = new float[3];
@ -292,14 +306,14 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) {
for (uint8_t Sensornumber = 0; Sensornumber < 12; for (uint8_t Sensornumber = 0; Sensornumber < 12;
Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for
// further processing // further processing
float* SunVectorBodyFrame = this[Sensornumber].getSunVectorBodyFrame(); float* SunVectorBodyFrame = SunVectorBodyFrame[Sensornumber];
SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0];
SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1];
SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2];
} }
for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) {
if (SUS[Sensornumber].getValidityNumber() == false) { if (getValidityNumber(Sensornumber) == false) {
counter = counter + 1; counter = counter + 1;
} // if the SUS data is not valid -> } // 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 switch (Sensornumber) { // find right basis matrix for each SUS
case 0: case 0:
BasisMatrixUse[c1][c2] = AcsParameters[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2];
break; break;
case 1: case 1:
BasisMatrixUse[c1][c2] = BasisMatrix1[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
break; break;
case 2: case 2:
BasisMatrixUse[c1][c2] = BasisMatrix2[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
break; break;
case 3: case 3:
BasisMatrixUse[c1][c2] = BasisMatrix3[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
break; break;
case 4: case 4:
BasisMatrixUse[c1][c2] = BasisMatrix4[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
break; break;
case 5: case 5:
BasisMatrixUse[c1][c2] = BasisMatrix5[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
break; break;
case 6: case 6:
BasisMatrixUse[c1][c2] = BasisMatrix6[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
break; break;
case 7: case 7:
BasisMatrixUse[c1][c2] = BasisMatrix7[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
break; break;
case 8: case 8:
BasisMatrixUse[c1][c2] = BasisMatrix8[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
break; break;
case 9: case 9:
BasisMatrixUse[c1][c2] = BasisMatrix9[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
break; break;
case 10: case 10:
BasisMatrixUse[c1][c2] = BasisMatrix10[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
break; break;
case 11: case 11:
BasisMatrixUse[c1][c2] = BasisMatrix11[c1][c2]; BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
break; 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 if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sum = 0; sum = 0;
for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) {
sum += SunVectorMatrixEIVE[i][Sensornumber]; if (getValidityNumber(Sensornumber)){
printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); sum += SunVectorMatrixEIVE[i][Sensornumber];
//printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]);
}
} }
// ToDo: decide on length on sun vector
SunVectorEIVE[i] = 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 // liegen, vermutlich aber an den falschen ChannelValues da die
// transformierten Sonnenvektoren jedes SUS plausibel sind // transformierten Sonnenvektoren jedes SUS plausibel sind
} }
VectorOperations<float>::normalize(SunVectorEIVE, SunVectorEIVE, 3);
} else { } else {
// No sus is valid // No sus is valid
throw std::invalid_argument("No sun sensor is valid"); // throw error throw std::invalid_argument("No sun sensor is valid"); // throw error

26
mission/controller/acs/SusConverter.h
View File

@ -16,24 +16,24 @@ class SunSensor {
public: public:
SunSensor() {} SunSensor() {}
void setSunSensorData(uint8_t Sensornumber); void setSunSensorData();
void checkSunSensorData(uint8_t Sensornumber); void checkSunSensorData(uint8_t Sensornumber);
void AngleCalculation(); void AngleCalculation(uint8_t Sensornumber);
void setCalibrationCoefficients(uint8_t Sensornumber); void setCalibrationCoefficients(uint8_t Sensornumber);
void Calibration(); void Calibration(uint8_t Sensornumber);
void CalculateSunVector(); void CalculateSunVector(uint8_t Sensornumber);
bool getValidityNumber() { return ValidityNumber; } bool getValidityNumber(uint8_t Sensornumber);
float* getSunVectorBodyFrame(); float* getSunVectorBodyFrame();
float* TransferSunVector(SunSensor SUS[12]); float* TransferSunVector();
private: private:
uint16_t ChannelValue[5]; //[Bit] uint16_t susChannelValues[12][4]; //[Bit]
float AlphaBetaRaw[2]; //[°] float AlphaBetaRaw[12][2]; //[°]
float AlphaBetaCalibrated[2]; //[°] float AlphaBetaCalibrated[12][2]; //[°]
float SunVectorBodyFrame[3]; //[-] float SunVectorBodyFrame[12][3]; //[-]
bool ValidityNumber = true; bool ValidityNumber[12] = true;
uint16_t ChannelValueCheckHigh = uint16_t ChannelValueCheckHigh =
4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check 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 HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in
// which cell a data point should be // which cell a data point should be
float CoeffAlpha[9][10]; float CoeffAlpha[12][9][10];
float CoeffBeta[9][10]; float CoeffBeta[12][9][10];
AcsParameters acsParameters; AcsParameters acsParameters;
}; };