fixed calculation of sun vector
This commit is contained in:
@ -210,45 +210,26 @@ void SensorProcessing::processSus(
|
||||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
if (sus0valid) {
|
||||
sus0valid = susConverter.checkSunSensorData(sus0Value);
|
||||
}
|
||||
if (sus1valid) {
|
||||
sus1valid = susConverter.checkSunSensorData(sus1Value);
|
||||
}
|
||||
if (sus2valid) {
|
||||
sus2valid = susConverter.checkSunSensorData(sus2Value);
|
||||
}
|
||||
if (sus3valid) {
|
||||
sus3valid = susConverter.checkSunSensorData(sus3Value);
|
||||
}
|
||||
if (sus4valid) {
|
||||
sus4valid = susConverter.checkSunSensorData(sus4Value);
|
||||
}
|
||||
if (sus5valid) {
|
||||
sus5valid = susConverter.checkSunSensorData(sus5Value);
|
||||
}
|
||||
if (sus6valid) {
|
||||
sus6valid = susConverter.checkSunSensorData(sus6Value);
|
||||
}
|
||||
if (sus7valid) {
|
||||
sus7valid = susConverter.checkSunSensorData(sus7Value);
|
||||
}
|
||||
if (sus8valid) {
|
||||
sus8valid = susConverter.checkSunSensorData(sus8Value);
|
||||
}
|
||||
if (sus9valid) {
|
||||
sus9valid = susConverter.checkSunSensorData(sus9Value);
|
||||
}
|
||||
if (sus10valid) {
|
||||
sus10valid = susConverter.checkSunSensorData(sus10Value);
|
||||
}
|
||||
if (sus11valid) {
|
||||
sus11valid = susConverter.checkSunSensorData(sus11Value);
|
||||
}
|
||||
|
||||
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
|
||||
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
|
||||
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
|
||||
susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
|
||||
susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
|
||||
susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
|
||||
susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
|
||||
susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
|
||||
susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
|
||||
susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
|
||||
susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
|
||||
susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
|
||||
susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
|
||||
susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
|
||||
|
||||
bool susValid[12] = {true, true, true, true, true, true, true, true, true, true, true, true};
|
||||
bool allInvalid =
|
||||
susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
|
||||
|
||||
if (allInvalid) {
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
@ -274,118 +255,78 @@ void SensorProcessing::processSus(
|
||||
}
|
||||
return;
|
||||
}
|
||||
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
|
||||
// Transformation into Geomtry Frame
|
||||
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
|
||||
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
|
||||
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
|
||||
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
|
||||
|
||||
if (sus0valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus0orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus1valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus2valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
|
||||
susParameters->sus2coeffBeta),
|
||||
sus2VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus3valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus3orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
|
||||
susParameters->sus3coeffBeta),
|
||||
sus3VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus4valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus4orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
|
||||
susParameters->sus4coeffBeta),
|
||||
sus4VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus5valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus5orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
|
||||
susParameters->sus5coeffBeta),
|
||||
sus5VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus6valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus6orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
|
||||
susParameters->sus6coeffBeta),
|
||||
sus6VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus7valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus7orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
|
||||
susParameters->sus7coeffBeta),
|
||||
sus7VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus8valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus8orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
|
||||
susParameters->sus8coeffBeta),
|
||||
sus8VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus9valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus9orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
|
||||
susParameters->sus9coeffBeta),
|
||||
sus9VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus10valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus10orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
|
||||
susParameters->sus10coeffBeta),
|
||||
sus10VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus11valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus11orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
|
||||
susParameters->sus11coeffBeta),
|
||||
sus11VecBody, 3, 3, 1);
|
||||
}
|
||||
float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
|
||||
{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
|
||||
{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
/* ------ Mean Value: susDirEst ------ */
|
||||
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
||||
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
||||
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
|
||||
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
|
||||
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
|
||||
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
|
||||
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
|
||||
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
|
||||
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
|
||||
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
|
||||
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
|
||||
if (susValid[0]) {
|
||||
susConverter.calculateSunVector(susVecSensor[0], sus0Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
|
||||
susVecBody[0], 3, 3, 1);
|
||||
}
|
||||
if (susValid[1]) {
|
||||
susConverter.calculateSunVector(susVecSensor[1], sus1Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
|
||||
susVecBody[1], 3, 3, 1);
|
||||
}
|
||||
if (susValid[2]) {
|
||||
susConverter.calculateSunVector(susVecSensor[2], sus2Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2],
|
||||
susVecBody[2], 3, 3, 1);
|
||||
}
|
||||
if (susValid[3]) {
|
||||
susConverter.calculateSunVector(susVecSensor[3], sus3Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3],
|
||||
susVecBody[3], 3, 3, 1);
|
||||
}
|
||||
if (susValid[4]) {
|
||||
susConverter.calculateSunVector(susVecSensor[4], sus4Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4],
|
||||
susVecBody[4], 3, 3, 1);
|
||||
}
|
||||
if (susValid[5]) {
|
||||
susConverter.calculateSunVector(susVecSensor[5], sus5Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5],
|
||||
susVecBody[5], 3, 3, 1);
|
||||
}
|
||||
if (susValid[6]) {
|
||||
susConverter.calculateSunVector(susVecSensor[6], sus6Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6],
|
||||
susVecBody[6], 3, 3, 1);
|
||||
}
|
||||
if (susValid[7]) {
|
||||
susConverter.calculateSunVector(susVecSensor[7], sus7Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7],
|
||||
susVecBody[7], 3, 3, 1);
|
||||
}
|
||||
if (susValid[8]) {
|
||||
susConverter.calculateSunVector(susVecSensor[8], sus8Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8],
|
||||
susVecBody[8], 3, 3, 1);
|
||||
}
|
||||
if (susValid[9]) {
|
||||
susConverter.calculateSunVector(susVecSensor[9], sus9Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9],
|
||||
susVecBody[9], 3, 3, 1);
|
||||
}
|
||||
if (susValid[10]) {
|
||||
susConverter.calculateSunVector(susVecSensor[10], sus10Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10],
|
||||
susVecBody[10], 3, 3, 1);
|
||||
}
|
||||
if (susValid[11]) {
|
||||
susConverter.calculateSunVector(susVecSensor[11], sus11Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11],
|
||||
susVecBody[11], 3, 3, 1);
|
||||
}
|
||||
|
||||
double susMeanValue[3] = {0, 0, 0};
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
if (validIds[i]) {
|
||||
susMeanValue[0] += susVecBody[0][i];
|
||||
susMeanValue[1] += susVecBody[1][i];
|
||||
susMeanValue[2] += susVecBody[2][i];
|
||||
}
|
||||
susMeanValue[0] += susVecBody[i][0];
|
||||
susMeanValue[1] += susVecBody[i][1];
|
||||
susMeanValue[2] += susVecBody[i][2];
|
||||
}
|
||||
double susVecTot[3] = {0.0, 0.0, 0.0};
|
||||
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
|
||||
@ -406,29 +347,29 @@ void SensorProcessing::processSus(
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float));
|
||||
susDataProcessed->sus2vec.setValid(sus2valid);
|
||||
std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float));
|
||||
susDataProcessed->sus3vec.setValid(sus3valid);
|
||||
std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float));
|
||||
susDataProcessed->sus4vec.setValid(sus4valid);
|
||||
std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float));
|
||||
susDataProcessed->sus5vec.setValid(sus5valid);
|
||||
std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float));
|
||||
susDataProcessed->sus6vec.setValid(sus6valid);
|
||||
std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float));
|
||||
susDataProcessed->sus7vec.setValid(sus7valid);
|
||||
std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float));
|
||||
susDataProcessed->sus8vec.setValid(sus8valid);
|
||||
std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float));
|
||||
susDataProcessed->sus9vec.setValid(sus9valid);
|
||||
std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float));
|
||||
susDataProcessed->sus10vec.setValid(sus10valid);
|
||||
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float));
|
||||
susDataProcessed->sus11vec.setValid(sus11valid);
|
||||
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
|
||||
susDataProcessed->susVecTot.setValid(true);
|
||||
|
||||
Reference in New Issue
Block a user