diff --git a/CHANGELOG.md b/CHANGELOG.md index 3d543d0f..669835d9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Fix sun vector calculation + # [v2.0.5] to be released - The dual lane assembly transition failed handler started new transitions towards the current mode diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 5ddc4d2f..c1361879 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -63,9 +63,9 @@ void ObjectFactory::produce(void* args) { StorageManagerIF* ipcStore; PersistentTmStores persistentStores; bool enableHkSets = false; - #if OBSW_ENABLE_PERIODIC_HK == 1 - enableHkSets = true; - #endif +#if OBSW_ENABLE_PERIODIC_HK == 1 + enableHkSets = true; +#endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, &tmStore, persistentStores); diff --git a/fsfw b/fsfw index 5eb9ee8b..f80c5980 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit f80c5980ea5bf84828a22dc6b4985a8747f85df4 diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..e3226e3b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x23: parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; + case 0x24: + parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..9e3a8b67 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; + float susBrightnessThreshold = 0.7; } susHandlingParameters; struct GyrHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4cc15a16..c00f522a 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -210,49 +210,56 @@ void SensorProcessing::processSus( sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[1] = sin(eclipticLongitude) * cos(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) { - sus0valid = susConverter.checkSunSensorData(sus0Value); + susBrightness[0] = susConverter.checkSunSensorData(sus0Value); } if (sus1valid) { - sus1valid = susConverter.checkSunSensorData(sus1Value); + susBrightness[1] = susConverter.checkSunSensorData(sus1Value); } if (sus2valid) { - sus2valid = susConverter.checkSunSensorData(sus2Value); + susBrightness[2] = susConverter.checkSunSensorData(sus2Value); } if (sus3valid) { - sus3valid = susConverter.checkSunSensorData(sus3Value); + susBrightness[3] = susConverter.checkSunSensorData(sus3Value); } if (sus4valid) { - sus4valid = susConverter.checkSunSensorData(sus4Value); + susBrightness[4] = susConverter.checkSunSensorData(sus4Value); } if (sus5valid) { - sus5valid = susConverter.checkSunSensorData(sus5Value); + susBrightness[5] = susConverter.checkSunSensorData(sus5Value); } if (sus6valid) { - sus6valid = susConverter.checkSunSensorData(sus6Value); + susBrightness[6] = susConverter.checkSunSensorData(sus6Value); } if (sus7valid) { - sus7valid = susConverter.checkSunSensorData(sus7Value); + susBrightness[7] = susConverter.checkSunSensorData(sus7Value); } if (sus8valid) { - sus8valid = susConverter.checkSunSensorData(sus8Value); + susBrightness[8] = susConverter.checkSunSensorData(sus8Value); } if (sus9valid) { - sus9valid = susConverter.checkSunSensorData(sus9Value); + susBrightness[9] = susConverter.checkSunSensorData(sus9Value); } if (sus10valid) { - sus10valid = susConverter.checkSunSensorData(sus10Value); + susBrightness[10] = susConverter.checkSunSensorData(sus10Value); } if (sus11valid) { - sus11valid = susConverter.checkSunSensorData(sus11Value); + susBrightness[11] = susConverter.checkSunSensorData(sus11Value); } - if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && - !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { + bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + bool allInvalid = + susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold); + + if (allInvalid) { { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { 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->sus1vec.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->sus10vec.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->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVecD, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVecD, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -274,118 +281,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::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::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::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0], + susVecBody[0], 3, 3, 1); + } + if (susValid[1]) { + susConverter.calculateSunVector(susVecSensor[1], sus1Value); + MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1], + susVecBody[1], 3, 3, 1); + } + if (susValid[2]) { + susConverter.calculateSunVector(susVecSensor[2], sus2Value); + MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2], + susVecBody[2], 3, 3, 1); + } + if (susValid[3]) { + susConverter.calculateSunVector(susVecSensor[3], sus3Value); + MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3], + susVecBody[3], 3, 3, 1); + } + if (susValid[4]) { + susConverter.calculateSunVector(susVecSensor[4], sus4Value); + MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4], + susVecBody[4], 3, 3, 1); + } + if (susValid[5]) { + susConverter.calculateSunVector(susVecSensor[5], sus5Value); + MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5], + susVecBody[5], 3, 3, 1); + } + if (susValid[6]) { + susConverter.calculateSunVector(susVecSensor[6], sus6Value); + MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6], + susVecBody[6], 3, 3, 1); + } + if (susValid[7]) { + susConverter.calculateSunVector(susVecSensor[7], sus7Value); + MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7], + susVecBody[7], 3, 3, 1); + } + if (susValid[8]) { + susConverter.calculateSunVector(susVecSensor[8], sus8Value); + MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8], + susVecBody[8], 3, 3, 1); + } + if (susValid[9]) { + susConverter.calculateSunVector(susVecSensor[9], sus9Value); + MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9], + susVecBody[9], 3, 3, 1); + } + if (susValid[10]) { + susConverter.calculateSunVector(susVecSensor[10], sus10Value); + MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10], + susVecBody[10], 3, 3, 1); + } + if (susValid[11]) { + susConverter.calculateSunVector(susVecSensor[11], sus11Value); + MatrixOperations::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::normalize(susMeanValue, susVecTot, 3); @@ -406,29 +373,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); diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1a645270..31cc0371 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,121 +1,64 @@ #include "SusConverter.h" -#include -#include -#include #include -#include - -bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { - if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || +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 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]) { - 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]) { - 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]) { - return false; + return 0; }; - susChannelValueSum = + uint64_t susChannelValueSum = 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); - if ((susChannelValueSum < susChannelValueSumHigh) && - (susChannelValueSum > susChannelValueSumLow)) { - return false; + if (susChannelValueSum < SUS_ALBEDO_CHECK) { + return 0; }; - return true; + return susChannelValueSum; } -void SusConverter::calcAngle(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; +bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12], + const float threshold) { + uint8_t maxBrightness = 0; + VectorOperations::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 - ch0 = susChannel[GNDREF] - susChannel[0]; - ch1 = susChannel[GNDREF] - susChannel[1]; - ch2 = susChannel[GNDREF] - susChannel[2]; - ch3 = susChannel[GNDREF] - susChannel[3]; + 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 - 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 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 - alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] -} - -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(); + sunVectorSensorFrame[0] = -xout; + sunVectorSensorFrame[1] = -yout; + sunVectorSensorFrame[2] = H; + VectorOperations::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3); } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 046b0ca8..db72f498 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -1,8 +1,4 @@ -#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - -#include -#include +#include #include "AcsParameters.h" @@ -10,41 +6,26 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(const uint16_t susChannel[6]); - - void calcAngle(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]); + uint64_t checkSunSensorData(const uint16_t susChannel[6]); + bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold); + void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]); 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; - uint16_t susChannelValueCheckHigh = - 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t susChannelValueCheckLow = - 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t susChannelValueSumHigh = - 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by - // the reflection of sunlight from the moon/earth - uint8_t susChannelValueSumLow = - 0; //[Bit]low borderline for check if the sun sensor is illuminated - // by the sun or by the reflection of sunlight from the moon/earth - uint8_t completeCellWidth = 140, - halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in - // which cell a data point should be - uint16_t susChannelValueSum = 0; + // =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; + // [Bit]low borderline for the channel values of one sun sensor for validity Check + static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0; + // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the + // reflection of sunlight from the moon/earth + static constexpr uint16_t SUS_ALBEDO_CHECK = 1000; + // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection + // of sunlight from the moon/earth + static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0; + + static constexpr float S = 0.03; // S=[mm] gap between diodes + static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture + static constexpr float H = 1; // H=[mm] distance between diodes and aperture AcsParameters acsParameters; }; - -#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */