65 lines
2.2 KiB
C++
65 lines
2.2 KiB
C++
#include "SusConverter.h"
|
|
|
|
#include <math.h>
|
|
|
|
uint64_t 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 0;
|
|
}
|
|
if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[1] > susChannel[GNDREF]) {
|
|
return 0;
|
|
};
|
|
if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[2] > susChannel[GNDREF]) {
|
|
return 0;
|
|
};
|
|
if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
|
|
susChannel[3] > susChannel[GNDREF]) {
|
|
return 0;
|
|
};
|
|
|
|
uint64_t susChannelValueSum =
|
|
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
|
|
if (susChannelValueSum < SUS_ALBEDO_CHECK) {
|
|
return 0;
|
|
};
|
|
return susChannelValueSum;
|
|
}
|
|
|
|
bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12],
|
|
const float threshold) {
|
|
uint8_t maxBrightness = 0;
|
|
VectorOperations<uint64_t>::maxValue(brightness, 12, &maxBrightness);
|
|
if (brightness[maxBrightness] == 0) {
|
|
return true;
|
|
}
|
|
for (uint8_t idx = 0; idx < 12; idx++) {
|
|
if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) {
|
|
susValid[idx] = false;
|
|
continue;
|
|
}
|
|
susValid[idx] = true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) {
|
|
// 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
|
|
sunVectorSensorFrame[0] = -xout;
|
|
sunVectorSensorFrame[1] = -yout;
|
|
sunVectorSensorFrame[2] = H;
|
|
VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
|
|
}
|