diff --git a/CHANGELOG.md b/CHANGELOG.md index f04e78ad..41d1b480 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,13 +20,19 @@ 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 ## 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. 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 27067dca..a683aada 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -10,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) { { @@ -186,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); @@ -523,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 @@ -557,6 +575,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; + + gpsSource = acs::GpsSource::GPS; } { PoolReadGuard pg(gpsDataProcessed); @@ -567,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); } } } @@ -594,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 6a167d83..77ff2869 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -32,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 @@ -39,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_ */