diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7bbefe08..7e49fcdd 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -33,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { + // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + double magIgrfModel[3] = {0.0, 0.0, 0.0}; + if (gpsValid) { + // 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.schmidtNormalization(); + 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(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, + gpsAltitude, timeOfMgmMeasurement, magIgrfModel); + } if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); mgmDataProcessed->setValidity(false, true); + std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); + mgmDataProcessed->magIgrfModel.setValid(gpsValid); } } return; @@ -138,20 +154,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ - double magIgrfModel[3] = {0.0, 0.0, 0.0}; - if (gpsValid) { - // 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.schmidtNormalization(); - 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(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, - gpsAltitude, timeOfMgmMeasurement, magIgrfModel); - } { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -187,6 +189,25 @@ void SensorProcessing::processSus( timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { + /* -------- Sun Model Direction (IJK frame) ------- */ + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + + // Julean Centuries + double sunIjkModel[3] = {0.0, 0.0, 0.0}; + 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; + + sunIjkModel[0] = cos(eclipticLongitude); + sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); + sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } @@ -229,22 +250,24 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.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)); + std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.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->sus11vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); susDataProcessed->setValidity(false, true); + std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); + susDataProcessed->sunIjkModel.setValid(true); } } return; @@ -263,16 +286,6 @@ void SensorProcessing::processSus( susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); - susDataProcessed->sus0vec.setValid(sus0valid); - if (!sus0valid) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus1valid) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], @@ -280,16 +293,6 @@ void SensorProcessing::processSus( susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); - susDataProcessed->sus1vec.setValid(sus1valid); - if (!sus1valid) { - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus2valid) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], @@ -397,27 +400,6 @@ void SensorProcessing::processSus( } } timeOfSavedSusDirEst = timeOfSusMeasurement; - - /* -------- Sun Model Direction (IJK frame) ------- */ - // if (useSunModel) eventuell - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - - // Julean Centuries - double sunIjkModel[3] = {0.0, 0.0, 0.0}; - 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; - - sunIjkModel[0] = cos(eclipticLongitude); - sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); - sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -474,6 +456,7 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { + double zeroVector[3] = {0.0, 0.0, 0.0}; std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 1a094dab..49c659b0 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -79,7 +79,6 @@ class SensorProcessing { double savedPosSatE[3] = {0.0, 0.0, 0.0}; double timeOfSavedPosSatE = 0.0; bool validSavedPosSatE = false; - const float zeroVector[3] = {0.0, 0.0, 0.0}; SusConverter susConverter; AcsParameters acsParameters;