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

little cleanup

Browse Source
This commit is contained in:
Marius Eggert 2023-06-16 21:16:39 +02:00
parent 0872fad7dc
commit 7cc13d2024
2 changed files with 31 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,54 +1,48 @@
#include "SusConverter.h" #include "SusConverter.h"
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <math.h>
#include <iostream>
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
susChannel[0] > susChannel[GNDREF]) { susChannel[0] > susChannel[GNDREF]) {
return false; return false;
} }
if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
susChannel[1] > susChannel[GNDREF]) { susChannel[1] > susChannel[GNDREF]) {
return false; return false;
}; };
if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
susChannel[2] > susChannel[GNDREF]) { susChannel[2] > susChannel[GNDREF]) {
return false; return false;
}; };
if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
susChannel[3] > susChannel[GNDREF]) { susChannel[3] > susChannel[GNDREF]) {
return false; return false;
}; };
susChannelValueSum = susChannelValueSum =
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
if ((susChannelValueSum < susChannelValueSumHigh) && if ((susChannelValueSum < SUS_CHANNEL_SUM_HIGH) &&
(susChannelValueSum > susChannelValueSumLow)) { (susChannelValueSum > SUS_CHANNEL_SUM_LOW)) {
return false; return false;
}; };
return true; return true;
} }
void SusConverter::calcAngle(const uint16_t susChannel[6]) { void SusConverter::calcAngle(const uint16_t susChannel[6]) {
float xout, yout;
float s = 0.03; // s=[mm] gap between diodes float s = 0.03; // s=[mm] gap between diodes
uint8_t d = 5; // d=[mm] edge length of the quadratic aperture float d = 5; // d=[mm] edge length of the quadratic aperture
uint8_t h = 1; // h=[mm] distance between diodes and aperture float h = 1; // h=[mm] distance between diodes and aperture
int ch0, ch1, ch2, ch3;
// Substract measurement values from GNDREF zero current threshold // Substract measurement values from GNDREF zero current threshold
ch0 = susChannel[GNDREF] - susChannel[0]; float ch0 = susChannel[GNDREF] - susChannel[0];
ch1 = susChannel[GNDREF] - susChannel[1]; float ch1 = susChannel[GNDREF] - susChannel[1];
ch2 = susChannel[GNDREF] - susChannel[2]; float ch2 = susChannel[GNDREF] - susChannel[2];
ch3 = susChannel[GNDREF] - susChannel[3]; float ch3 = susChannel[GNDREF] - susChannel[3];
// Calculation of x and y // Calculation of x and y
xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] float 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] float 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[0] = atan2(xout, h) * (180 / M_PI); //[°]
@ -99,15 +93,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
float* SusConverter::calculateSunVector() { float* SusConverter::calculateSunVector() {
// Calculate the normalized Sun Vector // Calculate the normalized Sun Vector
sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / sunVectorSensorFrame[0] = -tan(alphaBetaCalibrated[0] * (M_PI / 180));
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + sunVectorSensorFrame[1] = -tan(alphaBetaCalibrated[1] * (M_PI / 180));
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); sunVectorSensorFrame[2] = 1;
sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
(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) +
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
return sunVectorSensorFrame; return sunVectorSensorFrame;
} }

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

@ -29,19 +29,19 @@ class SusConverter {
returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK};
static const uint8_t GNDREF = 4; static const uint8_t GNDREF = 4;
uint16_t susChannelValueCheckHigh = // =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 static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096;
uint8_t susChannelValueCheckLow = // [Bit]low borderline for the channel values of one sun sensor for validity Check
0; //[Bit]low borderline for the channel values of one sun sensor for validity Check static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0;
uint16_t susChannelValueSumHigh = // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the
100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by // reflection of sunlight from the moon/earth
// the reflection of sunlight from the moon/earth static constexpr uint16_t SUS_CHANNEL_SUM_HIGH = 100;
uint8_t susChannelValueSumLow = // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection
0; //[Bit]low borderline for check if the sun sensor is illuminated // of sunlight from the moon/earth
// by the sun or by the reflection of sunlight from the moon/earth static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0;
uint8_t completeCellWidth = 140, // [°] 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 static const uint8_t completeCellWidth = 140, halfCellWidth = 70;
uint16_t susChannelValueSum = 0; uint16_t susChannelValueSum = 0;
AcsParameters acsParameters; AcsParameters acsParameters;