eive-obsw/mission/controller/acs/SusConverter.cpp

122 lines
5.4 KiB
C++
Raw Normal View History

2022-09-27 11:06:11 +02:00
#include "SusConverter.h"
2022-09-27 11:06:11 +02:00
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
2023-02-23 13:37:12 +01:00
#include <math.h>
2023-02-24 18:10:43 +01:00
#include <iostream>
2022-09-23 09:56:32 +02:00
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh ||
susChannel[0] > susChannel[GNDREF]) {
return false;
}
if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh ||
susChannel[1] > susChannel[GNDREF]) {
return false;
};
if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh ||
susChannel[2] > susChannel[GNDREF]) {
return false;
};
if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh ||
susChannel[3] > susChannel[GNDREF]) {
return false;
2022-09-23 09:56:32 +02:00
};
susChannelValueSum =
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
if ((susChannelValueSum < susChannelValueSumHigh) &&
(susChannelValueSum > susChannelValueSumLow)) {
return false;
2022-09-23 09:56:32 +02:00
};
return true;
2022-09-23 09:56:32 +02:00
}
void SusConverter::calcAngle(const uint16_t susChannel[6]) {
2022-09-27 11:06:11 +02:00
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
2022-09-23 09:56:32 +02:00
int ch0, ch1, ch2, ch3;
// Substract measurement values from GNDREF zero current threshold
ch0 = susChannel[GNDREF] - susChannel[0];
ch1 = susChannel[GNDREF] - susChannel[1];
ch2 = susChannel[GNDREF] - susChannel[2];
ch3 = susChannel[GNDREF] - susChannel[3];
2022-09-23 09:56:32 +02:00
// 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); //[°]
2022-09-23 09:56:32 +02:00
}
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
2023-02-21 10:50:08 +01:00
uint8_t index, k, l;
2022-09-23 09:56:32 +02:00
// while loop iterates above all calibration cells to use the different calibration functions in
// each cell
k = 0;
while (k < 3) {
k++;
2022-09-23 09:56:32 +02:00
l = 0;
while (l < 3) {
l++;
2022-09-23 09:56:32 +02:00
// if-condition to check in which cell the data point has to be
2023-02-21 10:50:08 +01:00
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))) {
2022-09-23 09:56:32 +02:00
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]; //[°]
2022-09-23 09:56:32 +02:00
}
}
}
}
float* SusConverter::calculateSunVector() {
2022-09-23 09:56:32 +02:00
// Calculate the normalized Sun Vector
sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
2022-12-01 15:56:55 +01:00
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
2022-12-01 15:56:55 +01:00
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[2] =
-(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
2022-12-01 15:56:55 +01:00
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
return sunVectorSensorFrame;
2022-09-23 09:56:32 +02:00
}
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();
2022-09-23 09:56:32 +02:00
}