111 lines
4.8 KiB
C++
111 lines
4.8 KiB
C++
#include "SusConverter.h"
|
|
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
|
|
if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[0] > susChannel[GNDREF]) {
|
|
return false;
|
|
}
|
|
if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[1] > susChannel[GNDREF]) {
|
|
return false;
|
|
};
|
|
if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[2] > susChannel[GNDREF]) {
|
|
return false;
|
|
};
|
|
if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[3] > susChannel[GNDREF]) {
|
|
return false;
|
|
};
|
|
|
|
susChannelValueSum =
|
|
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
|
|
if ((susChannelValueSum < SUS_CHANNEL_SUM_HIGH) &&
|
|
(susChannelValueSum > SUS_CHANNEL_SUM_LOW)) {
|
|
return false;
|
|
};
|
|
return true;
|
|
}
|
|
|
|
void SusConverter::calcAngle(const uint16_t susChannel[6]) {
|
|
float s = 0.03; // s=[mm] gap between diodes
|
|
float d = 5; // d=[mm] edge length of the quadratic aperture
|
|
float h = 1; // h=[mm] distance between diodes and aperture
|
|
// Substract measurement values from GNDREF zero current threshold
|
|
float ch0 = susChannel[GNDREF] - susChannel[0];
|
|
float ch1 = susChannel[GNDREF] - susChannel[1];
|
|
float ch2 = susChannel[GNDREF] - susChannel[2];
|
|
float ch3 = susChannel[GNDREF] - susChannel[3];
|
|
|
|
// Calculation of x and y
|
|
float xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
|
|
float 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 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
|
|
k = 0;
|
|
while (k < 3) {
|
|
k++;
|
|
l = 0;
|
|
while (l < 3) {
|
|
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)) &&
|
|
(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
|
|
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]; //[°]
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float* SusConverter::calculateSunVector() {
|
|
// Calculate the normalized Sun Vector
|
|
sunVectorSensorFrame[0] = -tan(alphaBetaCalibrated[0] * (M_PI / 180));
|
|
sunVectorSensorFrame[1] = -tan(alphaBetaCalibrated[1] * (M_PI / 180));
|
|
sunVectorSensorFrame[2] = 1;
|
|
VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
|
|
|
|
return sunVectorSensorFrame;
|
|
}
|
|
|
|
float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],
|
|
const float coeffAlpha[9][10],
|
|
const float coeffBeta[9][10]) {
|
|
calcAngle(susChannel);
|
|
calibration(coeffAlpha, coeffBeta);
|
|
return calculateSunVector();
|
|
}
|