combined SensorProcessing and SusConverter
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
This commit is contained in:
@ -6,191 +6,68 @@
|
||||
*/
|
||||
|
||||
#include "SusConverter.h"
|
||||
#include <math.h> //for atan2
|
||||
#include <iostream>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h> //for atan2
|
||||
|
||||
#include <iostream>
|
||||
|
||||
void SunSensor::checkSunSensorData(uint8_t susNumber) {
|
||||
uint16_t channelValueSum;
|
||||
|
||||
// Check individual channel values
|
||||
for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters
|
||||
|
||||
if (susChannelValues[susNumber][k] <= channelValueCheckLow ||
|
||||
susChannelValues[susNumber][k] > channelValueCheckHigh) { // Channel values out of range for 12 bit SUS
|
||||
// channel measurement range?
|
||||
validFlag[susNumber] = returnvalue::FAILED;
|
||||
/*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, susNumber, ChannelValue[k]);*/
|
||||
} else if (susChannelValues[susNumber][k] >
|
||||
susChannelValues[susNumber][4]) { // Channel values higher than zero current threshold GNDREF?
|
||||
validFlag[susNumber] = returnvalue::FAILED;
|
||||
/*printf(
|
||||
"The value of channel %i from sun sensor %i is higher than the zero current threshold "
|
||||
"GNDREF\n",
|
||||
k, susNumber);*/
|
||||
};
|
||||
bool SusConverter::checkSunSensorData(lp_vec_t<uint16_t, 6> susChannel) {
|
||||
if (susChannel.value[0] <= susChannelValueCheckLow ||
|
||||
susChannel.value[0] > susChannelValueCheckHigh ||
|
||||
susChannel.value[0] > susChannel.value[GNDREF]) {
|
||||
return false;
|
||||
}
|
||||
if (susChannel.value[1] <= susChannelValueCheckLow ||
|
||||
susChannel.value[1] > susChannelValueCheckHigh ||
|
||||
susChannel.value[1] > susChannel.value[GNDREF]) {
|
||||
return false;
|
||||
};
|
||||
if (susChannel.value[2] <= susChannelValueCheckLow ||
|
||||
susChannel.value[2] > susChannelValueCheckHigh ||
|
||||
susChannel.value[2] > susChannel.value[GNDREF]) {
|
||||
return false;
|
||||
};
|
||||
if (susChannel.value[3] <= susChannelValueCheckLow ||
|
||||
susChannel.value[3] > susChannelValueCheckHigh ||
|
||||
susChannel.value[3] > susChannel.value[GNDREF]) {
|
||||
return false;
|
||||
};
|
||||
|
||||
// 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 * susChannelValues[susNumber][4] - (susChannelValues[susNumber][0] +
|
||||
susChannelValues[susNumber][1] + susChannelValues[susNumber][2] +
|
||||
susChannelValues[susNumber][3]);
|
||||
if ((channelValueSum < channelValueSumHigh) && (channelValueSum > channelValueSumLow)) {
|
||||
validFlag[susNumber] = returnvalue::FAILED;
|
||||
//printf("Sun sensor %i is not illuminated by the sun\n", susNumber);
|
||||
susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] +
|
||||
susChannel.value[2] + susChannel.value[3]);
|
||||
if ((susChannelValueSum < susChannelValueSumHigh) &&
|
||||
(susChannelValueSum > susChannelValueSumLow)) {
|
||||
return false;
|
||||
};
|
||||
return true;
|
||||
}
|
||||
|
||||
void SunSensor::calcAngle(uint8_t susNumber) {
|
||||
void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> susChannel) {
|
||||
float xout, yout;
|
||||
float s = 0.03; // s=[mm] gap between diodes
|
||||
uint8_t d = 5; // d=[mm] edge length of the quadratic aperture
|
||||
uint8_t h = 1; // h=[mm] distance between diodes and aperture
|
||||
int ch0, ch1, ch2, ch3;
|
||||
// Substract measurement values from GNDREF zero current threshold
|
||||
ch0 = susChannelValues[susNumber][4] - susChannelValues[susNumber][0];
|
||||
ch1 = susChannelValues[susNumber][4] - susChannelValues[susNumber][1];
|
||||
ch2 = susChannelValues[susNumber][4] - susChannelValues[susNumber][2];
|
||||
ch3 = susChannelValues[susNumber][4] - susChannelValues[susNumber][3];
|
||||
ch0 = susChannel.value[GNDREF] - susChannel.value[0];
|
||||
ch1 = susChannel.value[GNDREF] - susChannel.value[1];
|
||||
ch2 = susChannel.value[GNDREF] - susChannel.value[2];
|
||||
ch3 = susChannel.value[GNDREF] - susChannel.value[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[susNumber][0] = atan2(xout, h) * (180 / M_PI); //[°]
|
||||
alphaBetaRaw[susNumber][1] = atan2(yout, h) * (180 / M_PI); //[°]
|
||||
alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°]
|
||||
alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°]
|
||||
}
|
||||
|
||||
void SunSensor::setCalibrationCoefficients(uint8_t susNumber) {
|
||||
switch (susNumber) { // 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[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][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[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column];
|
||||
coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column];
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void SunSensor::Calibration(uint8_t susNumber) {
|
||||
float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l;
|
||||
uint8_t index;
|
||||
|
||||
alpha_m = alphaBetaRaw[susNumber][0]; //[°]
|
||||
beta_m = alphaBetaRaw[susNumber][1]; //[°]
|
||||
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
|
||||
uint8_t index, k, l;
|
||||
|
||||
// while loop iterates above all calibration cells to use the different calibration functions in
|
||||
// each cell
|
||||
@ -201,68 +78,62 @@ void SunSensor::Calibration(uint8_t susNumber) {
|
||||
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))) {
|
||||
if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) &&
|
||||
alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) &&
|
||||
(alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) &&
|
||||
alphaBetaRaw[1] < ((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
|
||||
alphaBetaCalibrated[susNumber][0] =
|
||||
coeffAlpha[susNumber][index][0] + coeffAlpha[susNumber][index][1] * alpha_m + coeffAlpha[susNumber][index][2] * beta_m +
|
||||
coeffAlpha[susNumber][index][3] * alpha_m * alpha_m + coeffAlpha[susNumber][index][4] * alpha_m * beta_m +
|
||||
coeffAlpha[susNumber][index][5] * beta_m * beta_m +
|
||||
coeffAlpha[susNumber][index][6] * alpha_m * alpha_m * alpha_m +
|
||||
coeffAlpha[susNumber][index][7] * alpha_m * alpha_m * beta_m +
|
||||
coeffAlpha[susNumber][index][8] * alpha_m * beta_m * beta_m +
|
||||
coeffAlpha[susNumber][index][9] * beta_m * beta_m * beta_m; //[°]
|
||||
alphaBetaCalibrated[susNumber][1] =
|
||||
coeffBeta[susNumber][index][0] + coeffBeta[susNumber][index][1] * alpha_m +
|
||||
coeffBeta[susNumber][index][2] * beta_m + coeffBeta[susNumber][index][3] * alpha_m * alpha_m +
|
||||
coeffBeta[susNumber][index][4] * alpha_m * beta_m +
|
||||
coeffBeta[susNumber][index][5] * beta_m * beta_m +
|
||||
coeffBeta[susNumber][index][6] * alpha_m * alpha_m * alpha_m +
|
||||
coeffBeta[susNumber][index][7] * alpha_m * alpha_m * beta_m +
|
||||
coeffBeta[susNumber][index][8] * alpha_m * beta_m * beta_m +
|
||||
coeffBeta[susNumber][index][9] * beta_m * beta_m * beta_m; //[°]
|
||||
alphaBetaCalibrated[0] =
|
||||
coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +
|
||||
coeffAlpha[index][2] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°]
|
||||
alphaBetaCalibrated[1] =
|
||||
coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][2] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°]
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SunSensor::CalculateSunVector(uint8_t susNumber) {
|
||||
float alpha, beta;
|
||||
alpha = alphaBetaCalibrated[susNumber][0]; //[°]
|
||||
beta = alphaBetaCalibrated[susNumber][1]; //[°]
|
||||
|
||||
float* SusConverter::calculateSunVector() {
|
||||
// Calculate the normalized Sun Vector
|
||||
sunVectorBodyFrame[susNumber][0] =
|
||||
(tan(alpha * (M_PI / 180)) /
|
||||
(sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorBodyFrame[susNumber][1] =
|
||||
(tan(beta * (M_PI / 180)) /
|
||||
(sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorBodyFrame[susNumber][2] =
|
||||
(-1 /
|
||||
(sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorBodyFrame[0] = (tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
|
||||
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
|
||||
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorBodyFrame[1] = (tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
|
||||
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
|
||||
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorBodyFrame[2] =
|
||||
(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
|
||||
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
|
||||
return sunVectorBodyFrame;
|
||||
}
|
||||
|
||||
float* SunSensor::getSunVectorBodyFrame(uint8_t susNumber) {
|
||||
// return function for the sun vector in the body frame
|
||||
float* SunVectorBodyFrameReturn = 0;
|
||||
SunVectorBodyFrameReturn = new float[3];
|
||||
|
||||
SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[susNumber][0];
|
||||
SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[susNumber][1];
|
||||
SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[susNumber][2];
|
||||
|
||||
return SunVectorBodyFrameReturn;
|
||||
float* SusConverter::getSunVectorSensorFrame(lp_vec_t<uint16_t, 6> susChannel,
|
||||
const float coeffAlpha[9][10],
|
||||
const float coeffBeta[9][10]) {
|
||||
calcAngle(susChannel);
|
||||
calibration(coeffAlpha, coeffBeta);
|
||||
return calculateSunVector();
|
||||
}
|
||||
|
||||
bool SunSensor::getValidFlag(uint8_t susNumber) {
|
||||
return validFlag[susNumber];
|
||||
}
|
||||
bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; }
|
||||
|
||||
float* SunSensor::TransferSunVector() {
|
||||
float* SusConverter::TransferSunVector() {
|
||||
float* sunVectorEIVE = 0;
|
||||
sunVectorEIVE = new float[3];
|
||||
|
||||
@ -273,7 +144,7 @@ float* SunSensor::TransferSunVector() {
|
||||
|
||||
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
|
||||
// further processing
|
||||
float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber];
|
||||
sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0];
|
||||
sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1];
|
||||
@ -288,42 +159,53 @@ float* SunSensor::TransferSunVector() {
|
||||
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];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 1:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 2:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 3:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 4:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 5:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 6:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 7:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 8:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 9:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 10:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
|
||||
break;
|
||||
case 11:
|
||||
basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
|
||||
basisMatrixUse[c1][c2] =
|
||||
acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -343,9 +225,9 @@ float* SunSensor::TransferSunVector() {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
float sum = 0;
|
||||
for (uint8_t susNumber = 0; susNumber < 12; susNumber++) {
|
||||
if (getValidFlag(susNumber) == returnvalue::OK){
|
||||
if (getValidFlag(susNumber) == returnvalue::OK) {
|
||||
sum += sunVectorMatrixEIVE[i][susNumber];
|
||||
//printf("%f\n", SunVectorMatrixEIVE[i][susNumber]);
|
||||
// printf("%f\n", SunVectorMatrixEIVE[i][susNumber]);
|
||||
}
|
||||
}
|
||||
// ToDo: decide on length on sun vector
|
||||
@ -359,5 +241,3 @@ float* SunSensor::TransferSunVector() {
|
||||
|
||||
return sunVectorEIVE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user