diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e82cbb7d..0cb4f330 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -95,7 +95,8 @@ void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); @@ -148,7 +149,8 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); @@ -182,7 +184,8 @@ void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 8c89dda4..1baa4007 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,5 +1,6 @@ #include "SensorProcessing.h" +#include #include #include #include @@ -13,27 +14,35 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { - validMagField = false; - validGcLatitude = false; -} +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) + : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} SensorProcessing::~SensorProcessing() {} -bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, +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, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, double *magFieldEst, - bool *outputValid, double *magFieldModel, - bool *magFieldModelValid, double *magneticFieldVectorDerivative, - bool *magneticFieldVectorDerivativeValid) { + acsctrl::GpsDataProcessed *gpsDataProcessed, + const double gpsAltitude, bool gpsValid, + acsctrl::MgmDataProcessed *mgmDataProcessed) { if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { - *outputValid = false; - validMagField = false; - return false; + { + 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}, @@ -104,34 +113,26 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } + float mgmVecTot[3] = {0.0, 0.0, 0.0}; for (uint8_t i = 0; i < 3; i++) { - magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } - validMagField = true; - //-----------------------Mag Rate Computation --------------------------------------------------- + //-----------------------Mgm Rate Computation --------------------------------------------------- + float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; + bool mgmVecTotDerivativeValid = false; 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]; + if (timeOfSavedMagFieldEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; + savedMgmVecTot[i] = mgmVecTot[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 { + 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 @@ -139,12 +140,32 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const 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; + 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); + } } - - return true; } void SensorProcessing::processSus( @@ -155,9 +176,8 @@ void SensorProcessing::processSus( 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, double *sunDirEst, - bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed) { if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } @@ -197,142 +217,176 @@ void SensorProcessing::processSus( 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 sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, - sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, - sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, - sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - - if (sus0valid) { - MatrixOperations::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply( - susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, - susParameters->sus11coeffBeta), - sus11VecBody, 3, 3, 1); - } - - /* ------ 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]}}; - - double susMeanValue[3] = {0, 0, 0}; - for (uint8_t i = 0; i < 12; i++) { - if (validIds[i]) { - susMeanValue[0] += susVecBody[0][i]; - susMeanValue[1] += susVecBody[1][i]; - susMeanValue[2] += susVecBody[2][i]; + { + 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); } } - VectorOperations::normalize(susMeanValue, sunDirEst, 3); - *sunDirEstValid = 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); - for (uint8_t i = 0; i < 3; i++) { - sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff; - savedSunVector[i] = sunDirEst[i]; + if (timeOfSavedSusDirEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; + savedSusVecTot[i] = susVecTot[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) ------- */ @@ -340,6 +394,7 @@ void SensorProcessing::processSus( double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); // Julean Centuries + double sunIjkModel[3] = {0.0, 0.0, 0.0}; double JC2000 = JD2000 / 36525; double meanLongitude = @@ -351,11 +406,46 @@ void SensorProcessing::processSus( 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; + 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( @@ -366,133 +456,166 @@ void SensorProcessing::processGyr( 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, - double *satRatEst, bool *satRateEstValid) { - if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid && - !gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid && - !gyr3axYvalid && !gyr3axZvalid) { - *satRateEstValid = false; + 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}; - bool validUnit[4] = {false, false, false, false}; - if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) { + if (gyr0valid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); - validUnit[0] = true; } - if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) { + if (gyr1valid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); - validUnit[1] = true; } - if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) { + if (gyr2valid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); - validUnit[2] = true; } - if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) { + if (gyr3valid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); - validUnit[3] = true; } /* -------- SatRateEst: Middle Value ------- */ // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion - if (validUnit[0] && validUnit[2]) { + 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, satRatEst, 3); - } else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) { - if (validUnit[0]) { - satRatEst = gyr0ValueBody; - } else if (validUnit[2]) { - satRatEst = gyr2ValueBody; + 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 ((validUnit[1]) && (validUnit[3])) { + } else if (gyr1valid && gyr3valid) { double gyr13ValuesSum[3]; double gyr13ValuesMean[3]; VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); - if (validUnit[0]) { - satRatEst[0] = + if (gyr0valid) { + gyrVecTot[0] = ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - satRatEst[1] = + gyrVecTot[1] = ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - satRatEst[2] = + gyrVecTot[2] = ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else if (validUnit[2]) { - satRatEst[0] = + } else if (gyr2valid) { + gyrVecTot[0] = ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - satRatEst[1] = + gyrVecTot[1] = ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - satRatEst[2] = + gyrVecTot[2] = ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else - satRatEst = gyr13ValuesMean; - } else if (validUnit[1]) { - satRatEst = gyr1ValueBody; - } else if (validUnit[3]) { - satRatEst = gyr3ValueBody; + } 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); + } } - *satRateEstValid = true; } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude) { + 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; + 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; + 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, - ACS::OutputValues *outputValues, + 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(), &outputValues->gcLatitude, &outputValues->gdLongitude); + sensorValues->gpsSet.isValid(), gpsDataProcessed); - outputValues->mgmUpdated = 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, - outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value, - sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->magneticFieldVectorDerivative, - &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + 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(), @@ -507,10 +630,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, 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, - outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, - &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, - &outputValues->sunVectorDerivativeValid); - // VALID outputs ? + susDataProcessed); processGyr( sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), @@ -525,6 +645,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, 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, outputValues->satRateEst, - &outputValues->satRateEstValid); + &acsParameters->gyrHandlingParameters, gyrDataProcessed); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 50b78238..11d2ebeb 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,19 +23,21 @@ class SensorProcessing { SensorProcessing(AcsParameters *acsParameters_); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void process(timeval now, ACS::SensorValues *sensorValues, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, 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, + void 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 + acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, + bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed); void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, const uint16_t *sus2Value, bool sus2valid, @@ -47,9 +49,8 @@ class SensorProcessing { bool sus10valid, const uint16_t *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); + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed); void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, @@ -60,21 +61,23 @@ class SensorProcessing { const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, timeval timeOfGyrMeasurement, - const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst, - bool *satRateEstValid); + const AcsParameters::GyrHandlingParameters *gyrParameters, + acsctrl::GyrDataProcessed *gyrDataProcessed); void processStr(); void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, - double *gcLatitude, double *gdLongitude); + acsctrl::GpsDataProcessed *gpsDataProcessed); - double savedMagFieldEst[3]; + double savedMgmVecTot[3]; timeval timeOfSavedMagFieldEst; - double savedSunVector[3]; + double savedSusVecTot[3]; timeval timeOfSavedSusDirEst; bool validMagField; bool validGcLatitude; + const float zeroVector[3] = {0.0, 0.0, 0.0}; + SusConverter susConverter; AcsParameters acsParameters; };