diff --git a/CHANGELOG.md b/CHANGELOG.md index 8958a80c..a086ec9a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,17 +20,25 @@ will consitute of a breaking change warranting a new major release: ## Added +- SGP4 Propagator is now used for propagating the position of EIVE. It will only work once + a TLE has been uploaded with the newly added action command for the ACS Controller. In + return the actual GPS data will be ignored once SPG4 is running. However, by setting the + according parameter, the ACS Controller can be directed to ignore the SGP4 solution. - Skyview dataset for more GPS TM has been added - `PDEC_CONFIG_CORRUPTED` event which is triggered when the PDEC configuration does not match the expected configuration. P1 will contain the readback of the first word and P2 will contain the readback of the second word. ## Fixed + - The handling function of the GPS data is only called once per GPS read. This should remove the fake fix-has-changed events. ## Changed + - GPS Fix has changed event is no longer triggered for the EM +- MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available. + The stored value of the last timestep will now be reset, if no actual value is available. # [v6.3.0] 2023-08-03 diff --git a/CMakeLists.txt b/CMakeLists.txt index 908c89ac..6610b0a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -240,6 +240,9 @@ set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF) set(EIVE_ADD_LINUX_FILES OFF) set(FSFW_ADD_TMSTORAGE ON) +set(FSFW_ADD_COORDINATES ON) +set(FSFW_ADD_SGP4_PROPAGATOR ON) + # Analyse different OS and architecture/target options, determine BSP_PATH, # display information about compiler etc. pre_source_hw_os_config() diff --git a/fsfw b/fsfw index d575da85..796c7a9e 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d575da85407e029dabecaffa5368f0c9f1034941 +Subproject commit 796c7a9e377fa197e7e79b9a757d1b8e97419b8f diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 677ce37a..baed276d 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -42,6 +42,13 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_DETUMBLE_DETERIORATED = 21, }; +enum GpsSource : uint8_t { + NONE, + GPS, + GPS_EXTRAPOLATED, + SPG4, +}; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); @@ -64,6 +71,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG //! failed. //! P1: Missing information about magnetic field, P2: Missing information about rotational rate static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH); +//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old. +static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 08e8760d..febe385a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -22,7 +22,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) mekfData(this), ctrlValData(this), actuatorCmdData(this), - fusedRotRateData(this) {} + fusedRotRateData(this), + tleData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -62,6 +63,26 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t mekfLost = false; return HasActionsIF::EXECUTION_FINISHED; } + case UPDATE_TLE: { + if (size != 69 * 2) { + return INVALID_PARAMETERS; + } + ReturnValue_t result = navigation.updateTle(data, data + 69); + if (result != returnvalue::OK) { + PoolReadGuard pg(&tleData); + navigation.updateTle(tleData.line1.value, tleData.line2.value); + return result; + } + { + PoolReadGuard pg(&tleData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(tleData.line1.value, data, 69); + std::memcpy(tleData.line2.value, data + 69, 69); + tleData.setValidity(true, true); + } + } + return HasActionsIF::EXECUTION_FINISHED; + } default: { return HasActionsIF::INVALID_ACTION_ID; } @@ -146,12 +167,19 @@ void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); + ReturnValue_t result = navigation.useSpg4(now, &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); - ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); + result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -271,12 +299,19 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); + ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); + if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { + triggerEvent(acs::TLE_TOO_OLD); + tleTooOldFlag = true; + } else { + tleTooOldFlag = false; + } sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &fusedRotRateData); - ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); + result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -357,10 +392,17 @@ void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); + ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); + if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { + triggerEvent(acs::TLE_TOO_OLD); + tleTooOldFlag = true; + } else { + tleTooOldFlag = false; + } sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); + result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { mekfInvalidCounter++; @@ -735,6 +777,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); + localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0}); // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); @@ -758,6 +801,9 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); + // TLE Data + localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1); + localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2); return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a7dfbf6a..e03f2a55 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -61,6 +62,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; + bool tleTooOldFlag = false; uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; @@ -84,6 +86,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2; + static const DeviceCommandId_t UPDATE_TLE = 0x3; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. @@ -207,6 +210,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry altitude = PoolEntry(); PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); + PoolEntry gpsSource = PoolEntry(); // MEKF acsctrl::MekfData mekfData; @@ -234,6 +238,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rotRateParallel = PoolEntry(3); PoolEntry rotRateTotal = PoolEntry(3); + // TLE Dataset + acsctrl::TleData tleData; + PoolEntry line1 = PoolEntry(69); + PoolEntry line2 = PoolEntry(69); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index bcb1a7f4..2c97fa99 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -665,6 +665,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x3: parameterWrapper->set(gpsParameters.fdirAltitude); break; + case 0x4: + parameterWrapper->set(gpsParameters.useSpg4); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 0f2b53c9..ae33ac43 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -916,6 +916,7 @@ class AcsParameters : public HasParametersIF { double minimumFdirAltitude = 475 * 1e3; // [m] double maximumFdirAltitude = 575 * 1e3; // [m] double fdirAltitude = 525 * 1e3; // [m] + uint8_t useSpg4 = true; } gpsParameters; struct SunModelParameters { diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 7c822409..4726586b 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -44,3 +44,40 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, void Navigation::resetMekf(acsctrl::MekfData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } + +ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) { + double position[3] = {0, 0, 0}; + double velocity[3] = {0, 0, 0}; + ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0); + + if (result == returnvalue::OK) { + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + gpsDataProcessed->source = acs::GpsSource::SPG4; + gpsDataProcessed->source.setValid(true); + std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); + gpsDataProcessed->gpsPosition.setValid(true); + std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double)); + gpsDataProcessed->gpsVelocity.setValid(true); + } + } + } else { + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + gpsDataProcessed->source = acs::GpsSource::NONE; + gpsDataProcessed->source.setValid(true); + std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); + gpsDataProcessed->gpsPosition.setValid(false); + std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double)); + gpsDataProcessed->gpsVelocity.setValid(false); + } + } + } + return result; +} + +ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) { + return sgp4Propagator.initialize(line1, line2); +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index b567fbdd..2785ff2e 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -1,11 +1,13 @@ #ifndef NAVIGATION_H_ #define NAVIGATION_H_ -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include +#include +#include +#include +#include +#include +#include class Navigation { public: @@ -19,10 +21,14 @@ class Navigation { AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); + ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); + ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); + protected: private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; + Sgp4Propagator sgp4Propagator; }; #endif /* ACS_NAVIGATION_H_ */ diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 511cae35..a683aada 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,19 +1,5 @@ #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() {} SensorProcessing::~SensorProcessing() {} @@ -24,19 +10,20 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const bool mgm4valid, timeval timeOfMgmMeasurement, const AcsParameters::MgmHandlingParameters *mgmParameters, 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) { + bool gpsValid = false; + if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { Igrf13Model igrf13; 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); + gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel); + gpsValid = true; } if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { { @@ -54,6 +41,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgmDataProcessed->magIgrfModel.setValid(gpsValid); } } + std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot)); return; } float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, @@ -141,14 +129,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const 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) { - for (uint8_t i = 0; i < 3; i++) { - mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; - savedMgmVecTot[i] = mgmVecTot[i]; - mgmVecTotDerivativeValid = true; - } + if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and + VectorOperations::norm(savedMgmVecTot, 3) != 0) { + VectorOperations::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); + VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); + mgmVecTotDerivativeValid = true; } timeOfSavedMagFieldEst = timeOfMgmMeasurement; + std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and mgmDataProcessed->mgmVecTotDerivative.isValid()) { @@ -199,8 +187,8 @@ void SensorProcessing::processSus( double JC2000 = JD2000 / 36525.; double meanLongitude = - sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.; - double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; + sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * M_PI / 180.; + double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * M_PI / 180.; double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + sunModelParameters->p2 * sin(2 * meanAnomaly); @@ -277,6 +265,7 @@ void SensorProcessing::processSus( susDataProcessed->sunIjkModel.setValid(true); } } + std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot)); return; } @@ -365,13 +354,13 @@ 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) { - for (uint8_t i = 0; i < 3; i++) { - susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; - savedSusVecTot[i] = susVecTot[i]; - susVecTotDerivativeValid = true; - } + if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and + VectorOperations::norm(savedSusVecTot, 3) != 0) { + VectorOperations::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); + VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); + susVecTotDerivativeValid = true; } + std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); if (VectorOperations::norm(susVecTotDerivative, 3) != 0 and susDataProcessed->susVecTotDerivative.isValid()) { lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, @@ -535,14 +524,31 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { - double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, + // init variables + double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; - if (validGps) { - // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - gdLongitude = gpsLongitude * PI / 180.; - double latitudeRad = gpsLatitude * PI / 180.; - double eccentricityWgs84 = 0.0818195; - double factor = 1 - pow(eccentricityWgs84, 2); + uint8_t gpsSource = acs::GpsSource::NONE; + // We do not trust the GPS and therefore it shall die here if SPG4 is running + if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) { + MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, + gdLongitude, altitude); + double factor = 1 - pow(ECCENTRICITY_WGS84, 2); + gcLatitude = atan(factor * tan(gdLatitude)); + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + gpsDataProcessed->gdLongitude.value = gdLongitude; + gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->altitude.value = altitude; + gpsDataProcessed->setValidity(true, true); + } + } + return; + } else if (validGps) { + // Transforming from Degree to Radians and calculation geocentric latitude from geodetic + gdLongitude = gpsLongitude * M_PI / 180.; + double latitudeRad = gpsLatitude * M_PI / 180.; + double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(latitudeRad)); // Altitude FDIR @@ -569,6 +575,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; + + gpsSource = acs::GpsSource::GPS; } { PoolReadGuard pg(gpsDataProcessed); @@ -579,6 +587,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); gpsDataProcessed->setValidity(validGps, true); + gpsDataProcessed->source.value = gpsSource; + gpsDataProcessed->source.setValid(true); } } } @@ -606,11 +616,7 @@ 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, - sensorValues->gpsSet.altitude.value, - (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && - sensorValues->gpsSet.altitude.isValid()), - mgmDataProcessed); + now, &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(), diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 6dbc5d58..77ff2869 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,15 +1,23 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include +#include +#include +#include +#include +#include +#include #include -#include //uint8_t -#include /*purpose, timeval ?*/ +#include +#include +#include +#include +#include +#include +#include -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "SensorValues.h" -#include "SusConverter.h" -#include "eive/resultClassIds.h" +#include class SensorProcessing { public: @@ -25,6 +33,7 @@ class SensorProcessing { 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 @@ -32,8 +41,8 @@ class SensorProcessing { 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); + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed); void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, const uint16_t *sus2Value, bool sus2valid, diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b344451a..dd4c3f57 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -3,14 +3,15 @@ #include #include -#include +#include #include #include #include +#include #include -using namespace Math; +#include "fsfw/serviceinterface.h" template class MathOperations { @@ -114,6 +115,44 @@ class MathOperations { cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); } + + static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) { + /* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from + * cartesian coordinates in ECEF + * @param: x x-value of position vector [m] + * y y-value of position vector [m] + * z z-value of position vector [m] + * latitude geodetic latitude [rad] + * longitude longitude [rad] + * altitude altitude [m] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f + * Landis Markley and John L. Crassidis*/ + // From World Geodetic System the Earth Radii + double a = 6378137.0; // semimajor axis [m] + double b = 6356752.3142; // semiminor axis [m] + + // Calculation + double e2 = 1 - pow(b, 2) / pow(a, 2); + double epsilon2 = pow(a, 2) / pow(b, 2) - 1; + double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); + double p = std::abs(vector[2]) / epsilon2; + double s = pow(rho, 2) / (e2 * epsilon2); + double q = pow(p, 2) - pow(b, 2) + s; + double u = p / sqrt(q); + double v = pow(b, 2) * pow(u, 2) / q; + double P = 27 * v * s / q; + double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); + double t = (1 + Q + 1 / Q) / 6; + double c = sqrt(pow(u, 2) - 1 + 2 * t); + double w = (c - u) / 2; + double d = + sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); + double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); + latitude = asin((epsilon2 + 1) * d / N); + altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; + longitude = atan2(vector[1], vector[0]); + } + static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame * @param: time Current time @@ -140,7 +179,7 @@ class MathOperations { double FloorRest = floor(rest); double secOfDay = rest - FloorRest; secOfDay *= 86400; - gmst = secOfDay / 240 * PI / 180; + gmst = secOfDay / 240 * M_PI / 180; outputDcmEJ[0] = cos(gmst); outputDcmEJ[1] = sin(gmst); @@ -191,11 +230,11 @@ class MathOperations { double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // Earth Rotation angle double era = 0; - era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); // Greenwich Mean Sidereal Time double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); - double arcsecFactor = 1 * PI / (180 * 3600); + double arcsecFactor = 1 * M_PI / (180 * 3600); gmst2000 *= arcsecFactor; gmst2000 += era; @@ -247,7 +286,7 @@ class MathOperations { double de = 9.203 * arcsecFactor * cos(Om); // % true obliquity of the ecliptic eps p.71 (simplified) - double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180; nutation[0][0] = cos(dp); nutation[1][0] = cos(e + de) * sin(dp); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 86866c3f..b843ca0c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -20,6 +20,7 @@ enum SetIds : uint32_t { CTRL_VAL_DATA, ACTUATOR_CMD_DATA, FUSED_ROTATION_RATE_DATA, + TLE_SET, }; enum PoolIds : lp_id_t { @@ -85,6 +86,7 @@ enum PoolIds : lp_id_t { GYR_3_VEC, GYR_VEC_TOT, // GPS Processed + SOURCE, GC_LATITUDE, GD_LONGITUDE, ALTITUDE, @@ -108,6 +110,9 @@ enum PoolIds : lp_id_t { ROT_RATE_ORTHOGONAL, ROT_RATE_PARALLEL, ROT_RATE_TOTAL, + // TLE + TLE_LINE_1, + TLE_LINE_2, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; @@ -116,11 +121,12 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; -static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; +static constexpr uint8_t TLE_SET_ENTRIES = 2; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -239,6 +245,7 @@ class GpsDataProcessed : public StaticLocalDataSet { lp_var_t altitude = lp_var_t(sid.objectId, ALTITUDE, this); lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); + lp_var_t source = lp_var_t(sid.objectId, SOURCE, this); private: }; @@ -292,6 +299,16 @@ class FusedRotRateData : public StaticLocalDataSet { private: }; +class TleData : public StaticLocalDataSet { + public: + TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {} + + lp_vec_t line1 = lp_vec_t(sid.objectId, TLE_LINE_1, this); + lp_vec_t line2 = lp_vec_t(sid.objectId, TLE_LINE_1, this); + + private: +}; + } // namespace acsctrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */