#include "SensorProcessing.h" #include #include #include #include #include #include #include #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "Igrf13Model.h" #include "util/MathOperations.h" using namespace Math; SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} SensorProcessing::~SensorProcessing() {} void 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, acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { 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)); mgmDataProcessed->setValidity(false, true); } } return; } float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0}, mgm4ValueNoBias[3] = {0, 0, 0}; float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0}, mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; 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}; float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; if (mgm0valid) { VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias, mgm0ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, mgm0ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; } } if (mgm1valid) { VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias, mgm1ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, mgm1ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; } } if (mgm2valid) { VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias, mgm2ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, mgm2ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; } } if (mgm3valid) { VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias, mgm3ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, mgm3ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; } } if (mgm4valid) { VectorOperations::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, mgm4ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, mgm4ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } float mgmVecTot[3] = {0.0, 0.0, 0.0}; for (uint8_t i = 0; i < 3; i++) { mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } //-----------------------Mgm Rate Computation --------------------------------------------------- float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool mgmVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); if (timeOfSavedMagFieldEst.tv_sec != 0) { for (uint8_t i = 0; i < 3; i++) { mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; savedMgmVecTot[i] = mgmVecTot[i]; } } 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.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) { std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm0vec.setValid(mgm0valid); std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm1vec.setValid(mgm1valid); std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm2vec.setValid(mgm2valid); std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm3vec.setValid(mgm3valid); std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm4vec.setValid(mgm4valid); std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(float)); mgmDataProcessed->mgmVecTot.setValid(true); std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative, 3 * sizeof(float)); mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); mgmDataProcessed->setValidity(true, false); } } } void SensorProcessing::processSus( const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid, const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid, const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } if (sus1valid) { sus1valid = susConverter.checkSunSensorData(sus1Value); } if (sus2valid) { sus2valid = susConverter.checkSunSensorData(sus2Value); } if (sus3valid) { sus3valid = susConverter.checkSunSensorData(sus3Value); } if (sus4valid) { sus4valid = susConverter.checkSunSensorData(sus4Value); } if (sus5valid) { sus5valid = susConverter.checkSunSensorData(sus5Value); } if (sus6valid) { sus6valid = susConverter.checkSunSensorData(sus6Value); } if (sus7valid) { sus7valid = susConverter.checkSunSensorData(sus7Value); } if (sus8valid) { sus8valid = susConverter.checkSunSensorData(sus8Value); } if (sus9valid) { sus9valid = susConverter.checkSunSensorData(sus9Value); } if (sus10valid) { sus10valid = susConverter.checkSunSensorData(sus10Value); } if (sus11valid) { sus11valid = susConverter.checkSunSensorData(sus11Value); } if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { { 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)); susDataProcessed->setValidity(false, true); } } 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); } { 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], susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, 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], 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); } /* ------ 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]}}; float 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]; } } float susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); /* -------- Sun Derivatiative --------------------- */ float susVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool susVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); if (timeOfSavedSusDirEst.tv_sec != 0) { for (uint8_t i = 0; i < 3; i++) { susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; savedSusVecTot[i] = susVecTot[i]; } } 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) { std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); susDataProcessed->sus0vec.setValid(sus0valid); std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); susDataProcessed->sus1vec.setValid(sus1valid); std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); susDataProcessed->sus2vec.setValid(sus2valid); std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); susDataProcessed->sus3vec.setValid(sus3valid); std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); susDataProcessed->sus4vec.setValid(sus4valid); std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); susDataProcessed->sus5vec.setValid(sus5valid); std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); susDataProcessed->sus6vec.setValid(sus6valid); std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); susDataProcessed->sus7vec.setValid(sus7valid); std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); susDataProcessed->sus8vec.setValid(sus8valid); std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); susDataProcessed->sus9vec.setValid(sus9valid); std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); susDataProcessed->sus10vec.setValid(sus10valid); std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); susDataProcessed->sus11vec.setValid(sus11valid); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float)); susDataProcessed->susVecTot.setValid(true); std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative, 3 * sizeof(float)); susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); susDataProcessed->setValidity(true, false); } } } void SensorProcessing::processGyr( const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid, const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, acsctrl::GyrDataProcessed *gyrDataProcessed) { bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid); bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid); if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) { { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { 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)); std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); gyrDataProcessed->setValidity(false, true); } } return; } // Transforming Values to the Body Frame (actually it is the geometry frame atm) double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, gyr3ValueBody[3] = {0, 0, 0}; if (gyr0valid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); } if (gyr1valid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); } if (gyr2valid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); } if (gyr3valid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); } /* -------- SatRateEst: Middle Value ------- */ // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion double gyrVecTot[3] = {0.0, 0.0, 0.0}; if (gyr0valid && gyr2valid) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) { if (gyr0valid) { std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double)); } else if (gyr2valid) { std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double)); } } else if (gyr1valid && gyr3valid) { double gyr13ValuesSum[3]; double gyr13ValuesMean[3]; VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); if (gyr0valid) { gyrVecTot[0] = ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); gyrVecTot[1] = ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); gyrVecTot[2] = ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else if (gyr2valid) { gyrVecTot[0] = ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); gyrVecTot[1] = ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); gyrVecTot[2] = ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else { std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double)); } } else if (gyr1valid) { std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double)); } else if (gyr3valid) { std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double)); } { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double)); gyrDataProcessed->gyr0vec.setValid(gyr0valid); std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double)); gyrDataProcessed->setValidity(true, false); } } } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, const bool validGps, acsctrl::GpsDataProcessed *gpsDataProcessed) { // name to convert not process double gdLongitude, gcLatitude; 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; } { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; gpsDataProcessed->setValidity(validGps, validGps); if (!validGps) { gpsDataProcessed->gdLongitude.value = 0.0; gpsDataProcessed->gcLatitude.value = 0.0; } } } } void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, sensorValues->gpsSet.isValid(), gpsDataProcessed); processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), sensorValues->mgm1Rm3100Set.fieldStrengths.value, sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), sensorValues->mgm2Lis3Set.fieldStrengths.value, sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), sensorValues->mgm3Rm3100Set.fieldStrengths.value, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed); processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(), sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(), sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(), sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(), sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(), sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(), sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(), sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, susDataProcessed); processGyr( sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(), sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(), sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(), sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(), sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(), sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(), sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(), sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, &acsParameters->gyrHandlingParameters, gyrDataProcessed); }