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

sus-vector-fix-new #689

Merged
muellerr merged 8 commits from sus-vector-fix-new into v3.1.1-reduced-to-bare-minimum 2023-06-19 17:40:30 +02:00
Conversation 0 Commits 8 Files Changed 8 +171 -274
8 changed files with 171 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
- Fix sun vector calculation
# [v2.0.5] to be released # [v2.0.5] to be released
- The dual lane assembly transition failed handler started new transitions towards the current mode - The dual lane assembly transition failed handler started new transitions towards the current mode

2
fsfw

@ -1 +1 @@
Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 Subproject commit f80c5980ea5bf84828a22dc6b4985a8747f85df4

3
mission/controller/acs/AcsParameters.cpp
View File

@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x23: case 0x23:
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
break; break;
case 0x24:
parameterWrapper->set(susHandlingParameters.susBrightnessThreshold);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }

1
mission/controller/acs/AcsParameters.h
View File

@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF {
{116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
-0.000889232196185857, -0.00168429567131815}}; -0.000889232196185857, -0.00168429567131815}};
float susBrightnessThreshold = 0.7;
} susHandlingParameters; } susHandlingParameters;
struct GyrHandlingParameters { struct GyrHandlingParameters {

237
mission/controller/acs/SensorProcessing.cpp
View File

@ -210,49 +210,56 @@ void SensorProcessing::processSus(
sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
if (sus0valid) { if (sus0valid) {
sus0valid = susConverter.checkSunSensorData(sus0Value); susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
} }
if (sus1valid) { if (sus1valid) {
sus1valid = susConverter.checkSunSensorData(sus1Value); susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
} }
if (sus2valid) { if (sus2valid) {
sus2valid = susConverter.checkSunSensorData(sus2Value); susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
} }
if (sus3valid) { if (sus3valid) {
sus3valid = susConverter.checkSunSensorData(sus3Value); susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
} }
if (sus4valid) { if (sus4valid) {
sus4valid = susConverter.checkSunSensorData(sus4Value); susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
} }
if (sus5valid) { if (sus5valid) {
sus5valid = susConverter.checkSunSensorData(sus5Value); susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
} }
if (sus6valid) { if (sus6valid) {
sus6valid = susConverter.checkSunSensorData(sus6Value); susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
} }
if (sus7valid) { if (sus7valid) {
sus7valid = susConverter.checkSunSensorData(sus7Value); susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
} }
if (sus8valid) { if (sus8valid) {
sus8valid = susConverter.checkSunSensorData(sus8Value); susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
} }
if (sus9valid) { if (sus9valid) {
sus9valid = susConverter.checkSunSensorData(sus9Value); susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
} }
if (sus10valid) { if (sus10valid) {
sus10valid = susConverter.checkSunSensorData(sus10Value); susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
} }
if (sus11valid) { if (sus11valid) {
sus11valid = susConverter.checkSunSensorData(sus11Value); susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
} }
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
bool allInvalid =
susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
if (allInvalid) {
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
float zeroVec[3] = {0.0, 0.0, 0.0}; float zeroVec[3] = {0.0, 0.0, 0.0};
double zeroVecD[3] = {0, 0, 0};
std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
@ -265,8 +272,8 @@ void SensorProcessing::processSus(
std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->susVecTot.value, zeroVecD, 3 * sizeof(double));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVecD, 3 * sizeof(double));
susDataProcessed->setValidity(false, true); susDataProcessed->setValidity(false, true);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true); susDataProcessed->sunIjkModel.setValid(true);
@ -274,118 +281,78 @@ void SensorProcessing::processSus(
} }
return; 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) { float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
MatrixOperations<float>::multiply( {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
susParameters->sus0orientationMatrix[0], float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
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);
}
/* ------ Mean Value: susDirEst ------ */ if (susValid[0]) {
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, susConverter.calculateSunVector(susVecSensor[0], sus0Value);
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], susVecBody[0], 3, 3, 1);
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], }
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, if (susValid[1]) {
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], susConverter.calculateSunVector(susVecSensor[1], sus1Value);
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, susVecBody[1], 3, 3, 1);
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], }
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], if (susValid[2]) {
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[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}; double susMeanValue[3] = {0, 0, 0};
for (uint8_t i = 0; i < 12; i++) { for (uint8_t i = 0; i < 12; i++) {
if (validIds[i]) { susMeanValue[0] += susVecBody[i][0];
susMeanValue[0] += susVecBody[0][i]; susMeanValue[1] += susVecBody[i][1];
susMeanValue[1] += susVecBody[1][i]; susMeanValue[2] += susVecBody[i][2];
susMeanValue[2] += susVecBody[2][i];
}
} }
double susVecTot[3] = {0.0, 0.0, 0.0}; double susVecTot[3] = {0.0, 0.0, 0.0};
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3); VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
@ -406,29 +373,29 @@ void SensorProcessing::processSus(
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { 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); 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); 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); 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); 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); 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); 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); 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); 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); 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); 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); 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); susDataProcessed->sus11vec.setValid(sus11valid);
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
susDataProcessed->susVecTot.setValid(true); susDataProcessed->susVecTot.setValid(true);

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

@ -1,121 +1,64 @@
#include "SusConverter.h" #include "SusConverter.h"
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <math.h>
#include <iostream> uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh ||
susChannel[0] > susChannel[GNDREF]) { susChannel[0] > susChannel[GNDREF]) {
return false; return 0;
} }
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 0;
}; };
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 0;
}; };
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 0;
}; };
susChannelValueSum = uint64_t 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_ALBEDO_CHECK) {
(susChannelValueSum > susChannelValueSumLow)) { return 0;
return false;
}; };
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; 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::calcAngle(const uint16_t susChannel[6]) { void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) {
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
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); //[°] sunVectorSensorFrame[0] = -xout;
alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] sunVectorSensorFrame[1] = -yout;
} sunVectorSensorFrame[2] = H;
VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
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)) /
(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)) /
(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;
}
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();
} }

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

@ -1,8 +1,4 @@
#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #include <fsfw/globalfunctions/math/VectorOperations.h>
#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <stdint.h>
#include "AcsParameters.h" #include "AcsParameters.h"
@ -10,41 +6,26 @@ class SusConverter {
public: public:
SusConverter() {} SusConverter() {}
bool checkSunSensorData(const uint16_t susChannel[6]); uint64_t checkSunSensorData(const uint16_t susChannel[6]);
bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold);
void calcAngle(const uint16_t susChannel[6]); void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]);
void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]);
float* calculateSunVector();
float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10],
const float coeffBeta[9][10]);
private: private:
float alphaBetaRaw[2]; //[°]
float alphaBetaCalibrated[2]; //[°]
float sunVectorSensorFrame[3]; //[-]
bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
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_ALBEDO_CHECK = 1000;
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,
halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in static constexpr float S = 0.03; // S=[mm] gap between diodes
// which cell a data point should be static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture
uint16_t susChannelValueSum = 0; static constexpr float H = 1; // H=[mm] distance between diodes and aperture
AcsParameters acsParameters; AcsParameters acsParameters;
}; };
#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */