diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..1aac02c2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -122,6 +122,26 @@ void AcsController::performControlOperation() { } } + timeval timeAbsolute; + Clock::getClock_timeval(&timeAbsolute); + timeval timeRelative; + Clock::getClockMonotonic(&timeRelative); + + ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed); + if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { + triggerEvent(acs::TLE_TOO_OLD); + tleTooOldFlag = true; + } else if (result != Sgp4Propagator::TLE_TOO_OLD) { + tleTooOldFlag = false; + } + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, + &gyrDataProcessed, &fusedRotRateData); + result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData, &acsParameters); + switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4405790c..c0d2a128 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -7,7 +7,7 @@ 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, + bool mgm4valid, timeval timeAbsolute, double timeDelta, const AcsParameters::MgmHandlingParameters *mgmParameters, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed) { @@ -18,11 +18,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { Igrf13Model igrf13; igrf13.schmidtNormalization(); - igrf13.updateCoeffGH(timeOfMgmMeasurement); + igrf13.updateCoeffGH(timeAbsolute); + // ToDo // 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, - gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel); + gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel); gpsValid = true; } if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and @@ -129,14 +130,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const //-----------------------Mgm Rate Computation --------------------------------------------------- double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool mgmVecTotDerivativeValid = false; - double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and - VectorOperations::norm(savedMgmVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedMgmVecTot, 3) != 0) { VectorOperations::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); - VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); + VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative, + 3); mgmVecTotDerivativeValid = true; } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and @@ -177,11 +176,12 @@ void SensorProcessing::processSus( 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, + timeval timeAbsolute, double timeDelta, + const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { /* -------- Sun Model Direction (IJK frame) ------- */ - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + double JD2000 = MathOperations::convertUnixToJD2000(timeAbsolute); // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; @@ -354,11 +354,10 @@ void SensorProcessing::processSus( double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool susVecTotDerivativeValid = false; - double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and - VectorOperations::norm(savedSusVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedSusVecTot, 3) != 0) { VectorOperations::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); - VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); + VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative, + 3); susVecTotDerivativeValid = true; } std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); @@ -367,7 +366,6 @@ void SensorProcessing::processSus( lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, susParameters->susRateFilterWeight); } - timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -414,7 +412,7 @@ void SensorProcessing::processGyr( 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, + const AcsParameters::GyrHandlingParameters *gyrParameters, acsctrl::GyrDataProcessed *gyrDataProcessed) { bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); @@ -521,7 +519,7 @@ void SensorProcessing::processGyr( } void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, const double gpsUnixSeconds, + const double gpsAltitude, const double timeDelta, const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { @@ -563,18 +561,14 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); - if (validSavedPosSatE and - (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and - (gpsUnixSeconds - timeOfSavedPosSatE) > 0) { + if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); + VectorOperations::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3); } savedPosSatE[0] = posSatE[0]; savedPosSatE[1] = posSatE[1]; savedPosSatE[2] = posSatE[2]; - timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; gpsSource = acs::GpsSource::GPS; @@ -594,13 +588,20 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong } } -void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, +void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative, + ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); + double timeDelta = 0; + if (timeRelative.tv_sec != 0 and savedTimeRelative.tv_sec != 0) { + timeDelta = timevalOperations::toDouble(timeRelative - savedTimeRelative); + } + savedTimeRelative = timeRelative; + processGps( sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, @@ -617,7 +618,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->mgm3Rm3100Set.fieldStrengths.value, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), - now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed); + timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed, + mgmDataProcessed); processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), @@ -631,8 +633,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, 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); + timeAbsolute, timeDelta, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, susDataProcessed); processGyr( sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), @@ -646,7 +648,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, 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, + sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), &acsParameters->gyrHandlingParameters, gyrDataProcessed); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 77ff2869..70b78845 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -24,22 +24,21 @@ class SensorProcessing { SensorProcessing(); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, + void process(timeval timeAbsolute, timeval timeRelative, ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, - const AcsParameters *acsParameters); // Will call protected functions + acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); + private: static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; protected: - // short description needed for every function 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 float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta, const AcsParameters::MgmHandlingParameters *mgmParameters, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed); @@ -52,7 +51,7 @@ class SensorProcessing { 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, + timeval timeAbsolute, double timeDelta, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed); @@ -65,7 +64,6 @@ class SensorProcessing { 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); @@ -76,14 +74,11 @@ class SensorProcessing { void lowPassFilter(double *newValue, double *oldValue, const double weight); + timeval savedTimeRelative; double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; - timeval timeOfSavedMagFieldEst; double savedSusVecTot[3] = {0.0, 0.0, 0.0}; - timeval timeOfSavedSusDirEst; - bool validMagField = false; double savedPosSatE[3] = {0.0, 0.0, 0.0}; - double timeOfSavedPosSatE = 0.0; bool validSavedPosSatE = false; SusConverter susConverter;