diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 61f8092b..933800e2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,382 +6,441 @@ */ #include "SensorProcessing.h" -#include "Igrf13Model.h" -#include "util/MathOperations.h" -#include + #include #include #include #include #include +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "Igrf13Model.h" +#include "util/MathOperations.h" using namespace Math; // Thought: Maybe separate file for transforming of sensor values // into geometry frame and body frame -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : - savedMagFieldEst { 0, 0, 0 }{ - validMagField = false; - validGcLatitude = false; +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { + validMagField = false; + validGcLatitude = false; } -SensorProcessing::~SensorProcessing() { +SensorProcessing::~SensorProcessing() {} + +bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, + bool mgm1valid, const float *mgm2Value, bool mgm2valid, + const float *mgm3Value, bool mgm3valid, const float *mgm4Value, + bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, bool gpsValid, double *magFieldEst, + bool *outputValid, double *magFieldModel, + bool *magFieldModelValid, double *magneticFieldVectorDerivative, + bool *magneticFieldVectorDerivativeValid) { + if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { + *outputValid = false; + validMagField = false; + return false; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, + mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; + + bool validUnit[5] = {false, false, false, false, false}; + uint8_t validCount = 0; + if (mgm0valid) { + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, + mgm0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (mgm1valid) { + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, + mgm1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (mgm2valid) { + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, + mgm2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } + if (mgm3valid) { + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, + mgm3ValueBody, 3, 3, 1); + validCount += 1; + validUnit[3] = true; + } + if (mgm4valid) { + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, + mgm4ValueBody, 3, 3, 1); + validCount += 1; + validUnit[4] = true; + } + + /* -------- MagFieldEst: Middle Value ------- */ + float mgmValues[3][5] = { + {mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]}, + {mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]}, + {mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}}; + double mgmValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + mgmValidValues[0][j] = mgmValues[0][i]; + mgmValidValues[1][j] = mgmValues[1][i]; + mgmValidValues[2][j] = mgmValues[2][i]; + j += 1; + } + } + // Selection Sort + double mgmValidValuesSort[3][validCount]; + MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); + + uint8_t n = ceil(validCount / 2); + magFieldEst[0] = mgmValidValuesSort[0][n]; + magFieldEst[1] = mgmValidValuesSort[1][n]; + magFieldEst[2] = mgmValidValuesSort[2][n]; + validMagField = true; + + //-----------------------Mag Rate Computation --------------------------------------------------- + double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); + for (uint8_t i = 0; i < 3; i++) { + magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff; + savedMagFieldEst[i] = magFieldEst[i]; + } + + *magneticFieldVectorDerivativeValid = true; + if (timeOfSavedMagFieldEst.tv_sec == 0) { + magneticFieldVectorDerivative[0] = 0; + magneticFieldVectorDerivative[1] = 0; + magneticFieldVectorDerivative[2] = 0; + *magneticFieldVectorDerivativeValid = false; + } + + timeOfSavedMagFieldEst = timeOfMgmMeasurement; + + *outputValid = true; + + // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + if (!gpsValid) { + *magFieldModelValid = false; + } else { + // Should be existing class object which will be called and modified here. + Igrf13Model igrf13; + // So the line above should not be done here. Update: Can be done here as long updated coffs + // stored in acsParameters ? + igrf13.updateCoeffGH(timeOfMgmMeasurement); + // maybe put a condition here, to only update after a full day, this + // class function has around 700 steps to perform + igrf13.magFieldComp(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement, + magFieldModel); + *magFieldModelValid = false; + } + + return true; } -bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, - const float *mgm1Value, bool mgm1valid, const float *mgm2Value, - bool mgm2valid, const float *mgm3Value, bool mgm3valid, - const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, - const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, double *magFieldEst, bool *outputValid, - double *magFieldModel, bool*magFieldModelValid, - double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid) { +void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, + double *sunDirEst, bool *sunDirEstValid, + double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + susData.sus0.setValid(susConverter.checkSunSensorData(susData.sus0)); + susData.sus1.setValid(susConverter.checkSunSensorData(susData.sus1)); + susData.sus2.setValid(susConverter.checkSunSensorData(susData.sus2)); + susData.sus3.setValid(susConverter.checkSunSensorData(susData.sus3)); + susData.sus4.setValid(susConverter.checkSunSensorData(susData.sus4)); + susData.sus5.setValid(susConverter.checkSunSensorData(susData.sus5)); + susData.sus6.setValid(susConverter.checkSunSensorData(susData.sus6)); + susData.sus7.setValid(susConverter.checkSunSensorData(susData.sus7)); + susData.sus8.setValid(susConverter.checkSunSensorData(susData.sus8)); + susData.sus9.setValid(susConverter.checkSunSensorData(susData.sus9)); + susData.sus10.setValid(susConverter.checkSunSensorData(susData.sus10)); + susData.sus11.setValid(susConverter.checkSunSensorData(susData.sus11)); - if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { - *outputValid = false; - validMagField = false; - return false; - } - // Transforming Values to the Body Frame (actually it is the geometry frame atm) - float mgm0ValueBody[3] = {0,0,0}, mgm1ValueBody[3] = {0,0,0}, - mgm2ValueBody[3] = {0,0,0}, mgm3ValueBody[3] = {0,0,0}, - mgm4ValueBody[3] = {0,0,0}; + if (!susData.sus0.isValid() && !susData.sus1.isValid() && !susData.sus2.isValid() && + !susData.sus3.isValid() && !susData.sus4.isValid() && !susData.sus5.isValid() && + !susData.sus6.isValid() && !susData.sus7.isValid() && !susData.sus8.isValid() && + !susData.sus9.isValid() && !susData.sus10.isValid() && !susData.sus11.isValid()) { + *sunDirEstValid = false; + return; + } else { + // 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}; - bool validUnit[5] = {false, false, false, false, false}; - uint8_t validCount = 0; - if (mgm0valid) { - MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, mgm0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; - } - if (mgm1valid) { - MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, mgm1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; - } - if (mgm2valid) { - MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, mgm2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; - } - if (mgm3valid) { - MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, mgm3ValueBody, 3, 3, 1); - validCount += 1; - validUnit[3] = true; - } - if (mgm4valid) { - MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, mgm4ValueBody, 3, 3, 1); - validCount += 1; - validUnit[4] = true; - } + if (susData.sus0.isValid()) { + MatrixOperations::multiply( + susParameters->sus0orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha, + susParameters->sus0coeffBeta), + sus0VecBody, 3, 3, 1); + } + if (susData.sus1.isValid()) { + MatrixOperations::multiply( + susParameters->sus1orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha, + susParameters->sus1coeffBeta), + sus1VecBody, 3, 3, 1); + } + if (susData.sus2.isValid()) { + MatrixOperations::multiply( + susParameters->sus2orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha, + susParameters->sus2coeffBeta), + sus2VecBody, 3, 3, 1); + } + if (susData.sus3.isValid()) { + MatrixOperations::multiply( + susParameters->sus3orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha, + susParameters->sus3coeffBeta), + sus3VecBody, 3, 3, 1); + } + if (susData.sus4.isValid()) { + MatrixOperations::multiply( + susParameters->sus4orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha, + susParameters->sus4coeffBeta), + sus4VecBody, 3, 3, 1); + } + if (susData.sus5.isValid()) { + MatrixOperations::multiply( + susParameters->sus5orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha, + susParameters->sus5coeffBeta), + sus5VecBody, 3, 3, 1); + } + if (susData.sus6.isValid()) { + MatrixOperations::multiply( + susParameters->sus6orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha, + susParameters->sus6coeffBeta), + sus6VecBody, 3, 3, 1); + } + if (susData.sus7.isValid()) { + MatrixOperations::multiply( + susParameters->sus7orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha, + susParameters->sus7coeffBeta), + sus7VecBody, 3, 3, 1); + } + if (susData.sus8.isValid()) { + MatrixOperations::multiply( + susParameters->sus8orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha, + susParameters->sus8coeffBeta), + sus8VecBody, 3, 3, 1); + } + if (susData.sus9.isValid()) { + MatrixOperations::multiply( + susParameters->sus9orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha, + susParameters->sus9coeffBeta), + sus9VecBody, 3, 3, 1); + } + if (susData.sus10.isValid()) { + MatrixOperations::multiply( + susParameters->sus10orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha, + susParameters->sus10coeffBeta), + sus10VecBody, 3, 3, 1); + } + if (susData.sus11.isValid()) { + MatrixOperations::multiply( + susParameters->sus11orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus11, susParameters->sus11coeffAlpha, + susParameters->sus11coeffBeta), + sus11VecBody, 3, 3, 1); + } - /* -------- MagFieldEst: Middle Value ------- */ - float mgmValues[3][5] = { { mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], - mgm3ValueBody[0], mgm4ValueBody[0] }, { mgm0ValueBody[1], mgm1ValueBody[1], - mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1] }, { mgm0ValueBody[2], - mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2] } }; - double mgmValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - mgmValidValues[0][j] = mgmValues[0][i]; - mgmValidValues[1][j] = mgmValues[1][i]; - mgmValidValues[2][j] = mgmValues[2][i]; - j += 1; - } - } - //Selection Sort - double mgmValidValuesSort[3][validCount]; - MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); + /* ------ Mean Value: susDirEst ------ */ + // Timo already done + bool validIds[12] = {susData.sus0.isValid(), susData.sus1.isValid(), susData.sus2.isValid(), + susData.sus3.isValid(), susData.sus4.isValid(), susData.sus5.isValid(), + susData.sus6.isValid(), susData.sus7.isValid(), susData.sus8.isValid(), + susData.sus9.isValid(), susData.sus10.isValid(), susData.sus11.isValid()}; + 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]}}; - uint8_t n = ceil(validCount/2); - magFieldEst[0] = mgmValidValuesSort[0][n]; - magFieldEst[1] = mgmValidValuesSort[1][n]; - magFieldEst[2] = mgmValidValuesSort[2][n]; - validMagField = true; + double susMeanValue[3] = {0, 0, 0}; + uint8_t validSusCounter = 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]; + validSusCounter += 1; + } + } + double divisor = 1 / validSusCounter; + VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); + *sunDirEstValid = true; + } - //-----------------------Mag Rate Computation --------------------------------------------------- - double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - for (uint8_t i = 0; i < 3; i++) { - magneticFieldVectorDerivative[i] = (magFieldEst[i] - - savedMagFieldEst[i]) / timeDiff; - savedMagFieldEst[i] = magFieldEst[i]; - } + /* -------- Sun Derivatiative --------------------- */ - *magneticFieldVectorDerivativeValid = true; - if (timeOfSavedMagFieldEst.tv_sec == 0) { - magneticFieldVectorDerivative[0] = 0; - magneticFieldVectorDerivative[1] = 0; - magneticFieldVectorDerivative[2] = 0; - *magneticFieldVectorDerivativeValid = false; - } + double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); + for (uint8_t i = 0; i < 3; i++) { + sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff; + savedSunVector[i] = sunDirEst[i]; + } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; + *sunVectorDerivativeValid = true; + if (timeOfSavedSusDirEst.tv_sec == 0) { + sunVectorDerivative[0] = 0; + sunVectorDerivative[1] = 0; + sunVectorDerivative[2] = 0; + *sunVectorDerivativeValid = false; + } + timeOfSavedSusDirEst = timeOfSusMeasurement; - *outputValid = true; + /* -------- Sun Model Direction (IJK frame) ------- */ + // if (useSunModel) eventuell + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ - if (!gpsValid){ - *magFieldModelValid = false; - } - else{ - // Should be existing class object which will be called and modified here. - Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs -// stored in acsParameters ? - igrf13.updateCoeffGH(timeOfMgmMeasurement); - // maybe put a condition here, to only update after a full day, this - // class function has around 700 steps to perform - igrf13.magFieldComp(gpsLongitude, gpsLatitude, gpsAltitude, - timeOfMgmMeasurement, magFieldModel); - *magFieldModelValid = false; - } + // Julean Centuries + double JC2000 = JD2000 / 36525; - return true; + double meanLongitude = + (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180; + double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; + + double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + + sunModelParameters->p2 * sin(2 * meanAnomaly); + + double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; + + sunVectorInertial[0] = cos(eclipticLongitude); + sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon); + sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon); + + *sunVectorInertialValid = true; } -void SensorProcessing::processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, - const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, - const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, - const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, - const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, - const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, - timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, - double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid){ - - if(!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid - && !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid){ - *sunDirEstValid = false; - return; - } - else{ - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0ValueBody[3] = {0,0,0}, sus1ValueBody[3] = {0,0,0}, sus2ValueBody[3] = {0,0,0}, - sus3ValueBody[3] = {0,0,0}, sus4ValueBody[3] = {0,0,0}, sus5ValueBody[3] = {0,0,0}, - sus6ValueBody[3] = {0,0,0}, sus7ValueBody[3] = {0,0,0}, sus8ValueBody[3] = {0,0,0}, - sus9ValueBody[3] = {0,0,0}, sus10ValueBody[3] = {0,0,0}, sus11ValueBody[3] = {0,0,0}; - - if (sus0valid) { - MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], sus0Value, sus0ValueBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], sus1Value, sus1ValueBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], sus2Value, sus2ValueBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], sus3Value, sus3ValueBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], sus4Value, sus4ValueBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], sus5Value, sus5ValueBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], sus6Value, sus6ValueBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], sus7Value, sus7ValueBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], sus8Value, sus8ValueBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], sus9Value, sus9ValueBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], sus10Value, sus10ValueBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], sus11Value, sus11ValueBody, 3, 3, 1); - } - - /* ------ Mean Value: susDirEst ------ */ - // Timo already done - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susValuesBody[3][12] = {{sus0ValueBody[0], sus1ValueBody[0], sus2ValueBody[0], sus3ValueBody[0], sus4ValueBody[0], - sus5ValueBody[0], sus6ValueBody[0], sus7ValueBody[0], sus8ValueBody[0], sus9ValueBody[0], sus10ValueBody[0], sus11ValueBody[0]}, - {sus0ValueBody[1], sus1ValueBody[1], sus2ValueBody[1], sus3ValueBody[1], sus4ValueBody[1], - sus5ValueBody[1], sus6ValueBody[1], sus7ValueBody[1], sus8ValueBody[1], sus9ValueBody[1], sus10ValueBody[1], sus11ValueBody[1]}, - {sus0ValueBody[2], sus1ValueBody[2], sus2ValueBody[2], sus3ValueBody[2], sus4ValueBody[2], - sus5ValueBody[2], sus6ValueBody[2], sus7ValueBody[2], sus8ValueBody[2], sus9ValueBody[2], sus10ValueBody[2], sus11ValueBody[2]}}; - - double susMeanValue[3] = {0,0,0}; - uint8_t validSusCounter = 0; - for (uint8_t i = 0; i < 12; i++){ - if (validIds[i]){ - susMeanValue[0]+=susValuesBody[0][i]; - susMeanValue[1]+=susValuesBody[1][i]; - susMeanValue[2]+=susValuesBody[2][i]; - validSusCounter+=1; - } - } - double divisor = 1/validSusCounter; - VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); - *sunDirEstValid = true; - } - - /* -------- Sun Derivatiative --------------------- */ - - double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - for (uint8_t i = 0; i < 3; i++) { - sunVectorDerivative[i] = (sunDirEst[i] - - savedSunVector[i]) / timeDiff; - savedSunVector[i] = sunDirEst[i]; - } - - *sunVectorDerivativeValid = true; - if (timeOfSavedSusDirEst.tv_sec == 0) { - sunVectorDerivative[0] = 0; - sunVectorDerivative[1] = 0; - sunVectorDerivative[2] = 0; - *sunVectorDerivativeValid = false; - } - - timeOfSavedSusDirEst = timeOfSusMeasurement; - - /* -------- Sun Model Direction (IJK frame) ------- */ - // if (useSunModel) eventuell - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - - //Julean Centuries - double JC2000 = JD2000 / 36525; - - double meanLongitude = (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI /180; - double meanAnomaly = (sunModelParameters->m_0 - + sunModelParameters->dm * JC2000) * PI / 180.; - - double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) - + sunModelParameters->p2 * sin(2 * meanAnomaly); - - double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; - - - sunVectorInertial[0] = cos(eclipticLongitude); - sunVectorInertial[1] = sin(eclipticLongitude) - * cos(epsilon); - sunVectorInertial[2] = sin(eclipticLongitude) - * sin(epsilon); - - - *sunVectorInertialValid = true; -} - - void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid, - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid){ + const double rmu1Value[], bool rmu1valid, + const double rmu2Value[], bool rmu2valid, + timeval timeOfrmuMeasurement, + const AcsParameters::RmuHandlingParameters *rmuParameters, + double *satRatEst, bool *satRateEstValid) { + if (!rmu0valid && !rmu1valid && !rmu2valid) { + *satRateEstValid = false; + return; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0}; - if (!rmu0valid && !rmu1valid && !rmu2valid) { - *satRateEstValid = false; - return; - } - // Transforming Values to the Body Frame (actually it is the geometry frame atm) - double rmu0ValueBody[3] = {0,0,0}, rmu1ValueBody[3]= {0,0,0}, - rmu2ValueBody[3] = {0,0,0}; + bool validUnit[3] = {false, false, false}; + uint8_t validCount = 0; + if (rmu0valid) { + MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, + rmu0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (rmu1valid) { + MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, + rmu1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (rmu2valid) { + MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, + rmu2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } - bool validUnit[3] = {false, false, false}; - uint8_t validCount = 0; - if (rmu0valid) { - MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, rmu0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; - } - if (rmu1valid) { - MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, rmu1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; - } - if (rmu2valid) { - MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, rmu2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; - } - - /* -------- SatRateEst: Middle Value ------- */ - double rmuValues[3][3] = { { rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, { rmu0ValueBody[1], rmu1ValueBody[1], - rmu2ValueBody[1]}, { rmu0ValueBody[2], - rmu1ValueBody[2], rmu2ValueBody[2]} }; - double rmuValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - rmuValidValues[0][j] = rmuValues[0][i]; - rmuValidValues[1][j] = rmuValues[1][i]; - rmuValidValues[2][j] = rmuValues[2][i]; - j += 1; - } - } - //Selection Sort - double rmuValidValuesSort[3][validCount]; - MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); - - uint8_t n = ceil(validCount/2); - satRatEst[0] = rmuValidValuesSort[0][n]; - satRatEst[1] = rmuValidValuesSort[1][n]; - satRatEst[2] = rmuValidValuesSort[2][n]; - *satRateEstValid = true; + /* -------- SatRateEst: Middle Value ------- */ + double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, + {rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]}, + {rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}}; + double rmuValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + rmuValidValues[0][j] = rmuValues[0][i]; + rmuValidValues[1][j] = rmuValues[1][i]; + rmuValidValues[2][j] = rmuValues[2][i]; + j += 1; + } + } + // Selection Sort + double rmuValidValuesSort[3][validCount]; + MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); + uint8_t n = ceil(validCount / 2); + satRatEst[0] = rmuValidValuesSort[0][n]; + satRatEst[1] = rmuValidValuesSort[1][n]; + satRatEst[2] = rmuValidValuesSort[2][n]; + *satRateEstValid = true; } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude){ - // name to convert not process - if (validGps) { - // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - *gdLongitude = gps0longitude * PI / 180; - double latitudeRad = gps0latitude * PI / 180; - double eccentricityWgs84 = 0.0818195; - double factor = 1 - pow(eccentricityWgs84, 2); - *gcLatitude = atan(factor * tan(latitudeRad)); - validGcLatitude = true; - - } + const bool validGps, double *gcLatitude, double *gdLongitude) { + // name to convert not process + if (validGps) { + // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic + *gdLongitude = gps0longitude * PI / 180; + double latitudeRad = gps0latitude * PI / 180; + double eccentricityWgs84 = 0.0818195; + double factor = 1 - pow(eccentricityWgs84, 2); + *gcLatitude = atan(factor * tan(latitudeRad)); + validGcLatitude = true; + } } void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { + ACS::OutputValues *outputValues, + const AcsParameters *acsParameters) { + sensorValues->update(); + processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, + &outputValues->gcLatitude, &outputValues->gdLongitude); - sensorValues->update(); - processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, - sensorValues->gps0Valid, &outputValues->gcLatitude, &outputValues->gdLongitude); - - outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, - sensorValues->mgm1, sensorValues->mgm1Valid, - sensorValues->mgm2, sensorValues->mgm2Valid, - sensorValues->mgm3, sensorValues->mgm3Valid, - sensorValues->mgm4, sensorValues->mgm4Valid, now, - &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, - outputValues->gdLongitude, sensorValues->gps0altitude, - sensorValues->gps0Valid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->magneticFieldVectorDerivative, &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? - - processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, sensorValues->sus1Valid, - sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, sensorValues->sus3Valid, - sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, sensorValues->sus5Valid, - sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, sensorValues->sus7Valid, - sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, sensorValues->sus9Valid, - sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, sensorValues->sus11Valid, - now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // VALID outputs ? - - processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, sensorValues->rmu1Valid, - sensorValues->rmu2, sensorValues->rmu2Valid, now, &acsParameters->rmuHandlingParameters, - outputValues->satRateEst, &outputValues->satRateEstValid); + /*outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, + sensorValues->mgm1, sensorValues->mgm1Valid, + sensorValues->mgm2, sensorValues->mgm2Valid, + sensorValues->mgm3, sensorValues->mgm3Valid, + sensorValues->mgm4, sensorValues->mgm4Valid, now, + &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, + outputValues->gdLongitude, sensorValues->gps0altitude, + sensorValues->gps0Valid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->magneticFieldVectorDerivative, + &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, + sensorValues->sus1Valid, sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, + sensorValues->sus3Valid, sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, + sensorValues->sus5Valid, sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, + sensorValues->sus7Valid, sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, + sensorValues->sus9Valid, sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, + sensorValues->sus11Valid, now, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // + VALID outputs ? +*/ + processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, + sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, + &acsParameters->rmuHandlingParameters, outputValues->satRateEst, + &outputValues->satRateEstValid); } - diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 097c88ed..baa9c5b6 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -5,14 +5,16 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ -#include "AcsParameters.h" -#include "SensorValues.h" -#include "OutputValues.h" -#include "config/classIds.h" -#include //uint8_t -#include /*purpose, timeval ?*/ #include +#include //uint8_t +#include /*purpose, timeval ?*/ +#include "SusConverter.h" +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "OutputValues.h" +#include "SensorValues.h" +#include "config/classIds.h" /*Planned: * - Fusion of Sensor Measurements - @@ -26,65 +28,53 @@ * magField * SunDirEst*/ -class SensorProcessing{ -public: +class SensorProcessing { + public: + void reset(); - void reset(); + SensorProcessing(AcsParameters *acsParameters_); + virtual ~SensorProcessing(); - SensorProcessing(AcsParameters *acsParameters_); - virtual ~SensorProcessing(); + void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + const AcsParameters *acsParameters); // Will call protected functions + private: + protected: + // short description needed for every function + bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, + const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, + const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, + bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel, + bool *magFieldModelValid, double *magneticFieldVectorDerivative, + bool *magneticFieldVectorDerivativeValid); // Output - void process(timeval now, ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, - const AcsParameters *acsParameters); // Will call protected functions -private: + void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, + bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid); -protected: - // short description needed for every function - bool processMgm(const float *mgm0Value, bool mgm0valid, - const float *mgm1Value, bool mgm1valid, - const float *mgm2Value, bool mgm2valid, - const float *mgm3Value, bool mgm3valid, - const float *mgm4Value, bool mgm4valid, - timeval timeOfMgmMeasurement, - const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, - double *magFieldEst, bool *outputValid, - double *magFieldModel, bool*magFieldModelValid, - double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); //Output + void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu + const double rmu1Value[], bool rmu1valid, const double rmu2Value[], + bool rmu2valid, timeval timeOfrmuMeasurement, + const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst, + bool *satRateEstValid); - void processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, - const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, - const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, - const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, - const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, - const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, - timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, - double *sunVectorModel, bool *sunVectorModelValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid); + void processStr(); - void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid); - - void processStr(); - - void processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude); - - - double savedMagFieldEst[3]; - timeval timeOfSavedMagFieldEst; - double savedSunVector[3]; - timeval timeOfSavedSusDirEst; - bool validMagField; - bool validGcLatitude; + void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, + double *gcLatitude, double *gdLongitude); + double savedMagFieldEst[3]; + timeval timeOfSavedMagFieldEst; + double savedSunVector[3]; + timeval timeOfSavedSusDirEst; + bool validMagField; + bool validGcLatitude; + SusConverter susConverter; + AcsParameters acsParameters; }; #endif /*SENSORPROCESSING_H_*/ - diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 13f23808..7d8f5bd2 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -6,191 +6,68 @@ */ #include "SusConverter.h" -#include //for atan2 -#include -#include + #include #include +#include +#include //for atan2 +#include -void SunSensor::checkSunSensorData(uint8_t susNumber) { - uint16_t channelValueSum; - - // Check individual channel values - for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - - if (susChannelValues[susNumber][k] <= channelValueCheckLow || - susChannelValues[susNumber][k] > channelValueCheckHigh) { // Channel values out of range for 12 bit SUS - // channel measurement range? - validFlag[susNumber] = returnvalue::FAILED; - /*printf( - "The value of channel %i from sun sensor %i is not inside the borders of valid data with " - "a value of %i \n", - k, susNumber, ChannelValue[k]);*/ - } else if (susChannelValues[susNumber][k] > - susChannelValues[susNumber][4]) { // Channel values higher than zero current threshold GNDREF? - validFlag[susNumber] = returnvalue::FAILED; - /*printf( - "The value of channel %i from sun sensor %i is higher than the zero current threshold " - "GNDREF\n", - k, susNumber);*/ - }; +bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { + if (susChannel.value[0] <= susChannelValueCheckLow || + susChannel.value[0] > susChannelValueCheckHigh || + susChannel.value[0] > susChannel.value[GNDREF]) { + return false; + } + if (susChannel.value[1] <= susChannelValueCheckLow || + susChannel.value[1] > susChannelValueCheckHigh || + susChannel.value[1] > susChannel.value[GNDREF]) { + return false; + }; + if (susChannel.value[2] <= susChannelValueCheckLow || + susChannel.value[2] > susChannelValueCheckHigh || + susChannel.value[2] > susChannel.value[GNDREF]) { + return false; + }; + if (susChannel.value[3] <= susChannelValueCheckLow || + susChannel.value[3] > susChannelValueCheckHigh || + susChannel.value[3] > susChannel.value[GNDREF]) { + return false; }; - // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is - // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon - // reflection or earth albedo) - channelValueSum = - 4 * susChannelValues[susNumber][4] - (susChannelValues[susNumber][0] + - susChannelValues[susNumber][1] + susChannelValues[susNumber][2] + - susChannelValues[susNumber][3]); - if ((channelValueSum < channelValueSumHigh) && (channelValueSum > channelValueSumLow)) { - validFlag[susNumber] = returnvalue::FAILED; - //printf("Sun sensor %i is not illuminated by the sun\n", susNumber); + susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] + + susChannel.value[2] + susChannel.value[3]); + if ((susChannelValueSum < susChannelValueSumHigh) && + (susChannelValueSum > susChannelValueSumLow)) { + return false; }; + return true; } -void SunSensor::calcAngle(uint8_t susNumber) { +void SusConverter::calcAngle(lp_vec_t susChannel) { 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 - ch0 = susChannelValues[susNumber][4] - susChannelValues[susNumber][0]; - ch1 = susChannelValues[susNumber][4] - susChannelValues[susNumber][1]; - ch2 = susChannelValues[susNumber][4] - susChannelValues[susNumber][2]; - ch3 = susChannelValues[susNumber][4] - susChannelValues[susNumber][3]; + ch0 = susChannel.value[GNDREF] - susChannel.value[0]; + ch1 = susChannel.value[GNDREF] - susChannel.value[1]; + ch2 = susChannel.value[GNDREF] - susChannel.value[2]; + ch3 = susChannel.value[GNDREF] - susChannel.value[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] // Calculation of the angles - alphaBetaRaw[susNumber][0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[susNumber][1] = atan2(yout, h) * (180 / M_PI); //[°] + alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] + alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] } -void SunSensor::setCalibrationCoefficients(uint8_t susNumber) { - switch (susNumber) { // search for the correct calibration coefficients for each SUS - - case 0: - for (uint8_t row = 0; row < 9; - row++) { // save the correct coefficients in the right SUS class - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; - } - } - break; - - case 1: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; - } - } - break; - - case 2: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; - } - } - break; - - case 3: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; - } - } - break; - - case 4: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; - } - } - break; - - case 5: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; - } - } - break; - - case 6: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; - } - } - break; - - case 7: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; - } - } - break; - - case 8: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; - } - } - break; - - case 9: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; - } - } - break; - - case 10: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; - } - } - break; - - case 11: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; - } - } - break; - } -} - -void SunSensor::Calibration(uint8_t susNumber) { - float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; - uint8_t index; - - alpha_m = alphaBetaRaw[susNumber][0]; //[°] - beta_m = alphaBetaRaw[susNumber][1]; //[°] +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 @@ -201,68 +78,62 @@ void SunSensor::Calibration(uint8_t susNumber) { while (l < 3) { l = l + 1; // if-condition to check in which cell the data point has to be - if ((alpha_m > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && - alpha_m < ((completeCellWidth * (k / 3)) - halfCellWidth)) && - (beta_m > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && - beta_m < ((completeCellWidth * (l / 3)) - halfCellWidth))) { + 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 - // -> first cell has number 0 - alphaBetaCalibrated[susNumber][0] = - coeffAlpha[susNumber][index][0] + coeffAlpha[susNumber][index][1] * alpha_m + coeffAlpha[susNumber][index][2] * beta_m + - coeffAlpha[susNumber][index][3] * alpha_m * alpha_m + coeffAlpha[susNumber][index][4] * alpha_m * beta_m + - coeffAlpha[susNumber][index][5] * beta_m * beta_m + - coeffAlpha[susNumber][index][6] * alpha_m * alpha_m * alpha_m + - coeffAlpha[susNumber][index][7] * alpha_m * alpha_m * beta_m + - coeffAlpha[susNumber][index][8] * alpha_m * beta_m * beta_m + - coeffAlpha[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] - alphaBetaCalibrated[susNumber][1] = - coeffBeta[susNumber][index][0] + coeffBeta[susNumber][index][1] * alpha_m + - coeffBeta[susNumber][index][2] * beta_m + coeffBeta[susNumber][index][3] * alpha_m * alpha_m + - coeffBeta[susNumber][index][4] * alpha_m * beta_m + - coeffBeta[susNumber][index][5] * beta_m * beta_m + - coeffBeta[susNumber][index][6] * alpha_m * alpha_m * alpha_m + - coeffBeta[susNumber][index][7] * alpha_m * alpha_m * beta_m + - coeffBeta[susNumber][index][8] * alpha_m * beta_m * beta_m + - coeffBeta[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] + 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]; //[°] } } } } -void SunSensor::CalculateSunVector(uint8_t susNumber) { - float alpha, beta; - alpha = alphaBetaCalibrated[susNumber][0]; //[°] - beta = alphaBetaCalibrated[susNumber][1]; //[°] - +float* SusConverter::calculateSunVector() { // Calculate the normalized Sun Vector - sunVectorBodyFrame[susNumber][0] = - (tan(alpha * (M_PI / 180)) / - (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[susNumber][1] = - (tan(beta * (M_PI / 180)) / - (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[susNumber][2] = - (-1 / - (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); + sunVectorBodyFrame[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)))); + sunVectorBodyFrame[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)))); + sunVectorBodyFrame[2] = + (-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + + return sunVectorBodyFrame; } -float* SunSensor::getSunVectorBodyFrame(uint8_t susNumber) { - // return function for the sun vector in the body frame - float* SunVectorBodyFrameReturn = 0; - SunVectorBodyFrameReturn = new float[3]; - - SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[susNumber][0]; - SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[susNumber][1]; - SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[susNumber][2]; - - return SunVectorBodyFrameReturn; +float* SusConverter::getSunVectorSensorFrame(lp_vec_t susChannel, + const float coeffAlpha[9][10], + const float coeffBeta[9][10]) { + calcAngle(susChannel); + calibration(coeffAlpha, coeffBeta); + return calculateSunVector(); } -bool SunSensor::getValidFlag(uint8_t susNumber) { - return validFlag[susNumber]; -} +bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; } -float* SunSensor::TransferSunVector() { +float* SusConverter::TransferSunVector() { float* sunVectorEIVE = 0; sunVectorEIVE = new float[3]; @@ -273,7 +144,7 @@ float* SunSensor::TransferSunVector() { for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { // save the sun vector of each SUS in their body frame into an array for - // further processing + // further processing float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber]; sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; @@ -288,42 +159,53 @@ float* SunSensor::TransferSunVector() { for (uint8_t c1 = 0; c1 < 3; c1++) { for (uint8_t c2 = 0; c2 < 3; c2++) { switch (susNumber) { - case 0: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -343,9 +225,9 @@ float* SunSensor::TransferSunVector() { for (uint8_t i = 0; i < 3; i++) { float sum = 0; for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { - if (getValidFlag(susNumber) == returnvalue::OK){ + if (getValidFlag(susNumber) == returnvalue::OK) { sum += sunVectorMatrixEIVE[i][susNumber]; - //printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); + // printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); } } // ToDo: decide on length on sun vector @@ -359,5 +241,3 @@ float* SunSensor::TransferSunVector() { return sunVectorEIVE; } - - diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index bfec1dfe..32f9e954 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -8,69 +8,54 @@ #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#include "AcsParameters.h" +#include #include -class SunSensor { - public: - SunSensor() {} +#include "AcsParameters.h" - void checkSunSensorData(uint8_t susNumber); - void calcAngle(uint8_t susNumber); - void setCalibrationCoefficients(uint8_t susNumber); - void Calibration(uint8_t susNumber); - void CalculateSunVector(uint8_t susNumber); +class SusConverter { + public: + SusConverter() {} + + bool checkSunSensorData(lp_vec_t susChannel); + + void calcAngle(lp_vec_t susChannel); + void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); + float* calculateSunVector(); bool getValidFlag(uint8_t susNumber); - float* getSunVectorBodyFrame(uint8_t susNumber); + float* getSunVectorSensorFrame(lp_vec_t susChannel, const float coeffAlpha[9][10], + const float coeffBeta[9][10]); float* TransferSunVector(); private: - // ToDo: remove statics and replace with actual data - uint16_t susChannelValues[12][4] = { - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}}; //[Bit] - float alphaBetaRaw[12][2]; //[°] - float alphaBetaCalibrated[12][2]; //[°] - float sunVectorBodyFrame[12][3]; //[-] + float alphaBetaRaw[2]; //[°] + // float coeffAlpha[9][10]; + // float coeffBeta[9][10]; + float alphaBetaCalibrated[2]; //[°] + float sunVectorBodyFrame[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}; + 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}; - uint16_t channelValueCheckHigh = + 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 channelValueCheckLow = + uint8_t susChannelValueCheckLow = 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t channelValueSumHigh = + 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 channelValueSumLow = + 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 - - float coeffAlpha[12][9][10]; - float coeffBeta[12][9][10]; + uint16_t susChannelValueSum = 0; AcsParameters acsParameters; }; - #endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */