From 080b729f116ad5dc205386785ec1ec877c9eace3 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 3 Aug 2023 14:41:35 +0200 Subject: [PATCH 01/28] wow so much progress --- mission/controller/acs/Navigation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 7c822409..7d60a531 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,5 +1,6 @@ #include "Navigation.h" +#include #include #include #include From ef8409d4a8129465e2da644f805a067b17bf4968 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 09:38:35 +0200 Subject: [PATCH 02/28] apparently FLP is part of the cool kids who store their paramenters as datasets --- .../controllerdefinitions/AcsCtrlDefinitions.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 86866c3f..e8fb2b53 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 { @@ -108,6 +109,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; @@ -121,6 +125,7 @@ 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. @@ -292,6 +297,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_ */ From ad6e0e994621d65300df9609824a3490c2a80f53 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 10:31:26 +0200 Subject: [PATCH 03/28] add source flag --- .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index e8fb2b53..d25d41a3 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -86,6 +86,7 @@ enum PoolIds : lp_id_t { GYR_3_VEC, GYR_VEC_TOT, // GPS Processed + SOURCE, GC_LATITUDE, GD_LONGITUDE, ALTITUDE, @@ -120,7 +121,7 @@ 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; @@ -244,8 +245,9 @@ 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: + private: }; class MekfData : public StaticLocalDataSet { From 4b90d2644546f70a73a24ec5d57bbdc8c024b1fc Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 10:39:20 +0200 Subject: [PATCH 04/28] add source flag for GPS data --- mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index d25d41a3..b843ca0c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -245,9 +245,9 @@ 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) + lp_var_t source = lp_var_t(sid.objectId, SOURCE, this); - private: + private: }; class MekfData : public StaticLocalDataSet { From f9befaebd0aacc41602900dad499c87f8803ffc4 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 10:43:00 +0200 Subject: [PATCH 05/28] action command to update TLE --- mission/controller/AcsController.cpp | 26 +++++++++++++++++++++++++- mission/controller/AcsController.h | 6 ++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 08e8760d..02c0e094 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; } @@ -758,6 +779,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..ef7817b9 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -84,6 +84,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. @@ -234,6 +235,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); }; From 2a0b139f706b8e997f2cd5563b90056edb51f9ff Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 11:23:10 +0200 Subject: [PATCH 06/28] spg4 into navigation --- mission/controller/acs/Navigation.cpp | 33 ++++++++++++++++++++++++++- mission/controller/acs/Navigation.h | 15 ++++++++---- 2 files changed, 42 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 7d60a531..3b7d1cc1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,6 +1,5 @@ #include "Navigation.h" -#include #include #include #include @@ -45,3 +44,35 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, void Navigation::resetMekf(acsctrl::MekfData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } + +void 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) { + 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) { + 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); + } + } + } +} + +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..4dd2ceef 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -1,11 +1,12 @@ #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 class Navigation { public: @@ -19,10 +20,14 @@ class Navigation { AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); + void 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_ */ From e261d3609b0586a38a9b3e6d3e8238955be953df Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 11:36:30 +0200 Subject: [PATCH 07/28] use spg4 --- mission/controller/AcsController.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 02c0e094..73000234 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -167,6 +167,7 @@ void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); + navigation.useSpg4(now, &gpsDataProcessed); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, @@ -292,6 +293,7 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); + navigation.useSpg4(now, &gpsDataProcessed); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, @@ -378,6 +380,7 @@ void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); + navigation.useSpg4(now, &gpsDataProcessed); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, From 2b03c413054f3cb76a3ff17eeb21e87a90324236 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 13:35:32 +0200 Subject: [PATCH 08/28] i guess i had to edit it here, thanks mr cmake god --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) 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() From a81d911f8e6f5df56a2c070324f47b90e4870ce2 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 13:36:12 +0200 Subject: [PATCH 09/28] init source pool var --- mission/controller/AcsController.cpp | 1 + mission/controller/AcsController.h | 1 + 2 files changed, 2 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 73000234..6ae108c5 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -759,6 +759,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); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ef7817b9..a40dba3b 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -208,6 +208,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; From e631ab55e614051e905bebdd55dd4a5bd74c8b97 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 13:36:40 +0200 Subject: [PATCH 10/28] enum for gps source --- mission/acs/defs.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 677ce37a..518ef59e 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -42,6 +42,12 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_DETUMBLE_DETERIORATED = 21, }; +enum GpsSource : uint8_t { + NONE, + GPS, + 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); From 04c9013c6456a0a64080edcf5587e6cbfcf4f4cd Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 13:37:05 +0200 Subject: [PATCH 11/28] feedback of spg4 usage to dataset --- mission/controller/acs/Navigation.cpp | 4 ++++ mission/controller/acs/Navigation.h | 1 + 2 files changed, 5 insertions(+) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 3b7d1cc1..61f9bda4 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -54,6 +54,8 @@ void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcesse { 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)); @@ -64,6 +66,8 @@ void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcesse { 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)); diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 4dd2ceef..bc39555f 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -2,6 +2,7 @@ #define NAVIGATION_H_ #include +#include #include #include #include From 947eef7170a1acc1abbb2a6bc626566253e6e1f6 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 13:45:06 +0200 Subject: [PATCH 12/28] just in case we ever do this --- mission/acs/defs.h | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 518ef59e..3f0b31c4 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -45,6 +45,7 @@ enum SafeModeStrategy : uint8_t { enum GpsSource : uint8_t { NONE, GPS, + PROPAGATION, SPG4, }; From dfa20545e48b4fa2cf0ec7f3ce03f6d451a17725 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 16:43:31 +0200 Subject: [PATCH 13/28] yes ofc i know what every one of these equations does --- mission/controller/acs/util/MathOperations.h | 51 +++++++++++++++++--- 1 file changed, 44 insertions(+), 7 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b344451a..e21a5f3a 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -3,15 +3,14 @@ #include #include -#include +#include #include #include #include +#include #include -using namespace Math; - template class MathOperations { public: @@ -114,6 +113,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 + pow(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4, 1 / 2)); + 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 +177,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 +228,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 +284,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); From fa3d5cbc3ed1471b07fc27bdc98f4bccf4f2b969 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 7 Aug 2023 16:44:03 +0200 Subject: [PATCH 14/28] handling of which GPS result to use --- mission/controller/acs/SensorProcessing.cpp | 30 +++++++++++++++++---- mission/controller/acs/SensorProcessing.h | 15 +++++------ 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 511cae35..d7e6794e 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -535,14 +535,30 @@ 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 + 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) { + 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); + } + } + } else if (validGps) { + // Transforming from Degree to Radians and calculation geocentric latitude from geodetic gdLongitude = gpsLongitude * PI / 180.; double latitudeRad = gpsLatitude * PI / 180.; - double eccentricityWgs84 = 0.0818195; - double factor = 1 - pow(eccentricityWgs84, 2); + double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(latitudeRad)); // Altitude FDIR @@ -569,6 +585,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; + + gpsSource = acs::GpsSource::GPS; } { PoolReadGuard pg(gpsDataProcessed); @@ -579,6 +597,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); } } } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 6dbc5d58..7bd88fa2 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,15 +1,13 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include #include -#include //uint8_t -#include /*purpose, timeval ?*/ - -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "SensorValues.h" -#include "SusConverter.h" -#include "eive/resultClassIds.h" +#include +#include +#include +#include +#include class SensorProcessing { public: @@ -25,6 +23,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 From 11578b9d9a527ca04d4f9fda7f174e2828632ecb Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 8 Aug 2023 13:47:44 +0200 Subject: [PATCH 15/28] i hate myself --- mission/controller/acs/util/MathOperations.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index e21a5f3a..555f38de 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -139,12 +139,12 @@ class MathOperations { 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 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 + pow(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4, 1 / 2)); + 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; From 8da08bd32846123ad150fd3984df37895d443944 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 8 Aug 2023 14:24:26 +0200 Subject: [PATCH 16/28] fix --- mission/controller/acs/SensorProcessing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index d7e6794e..c93c6027 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -554,6 +554,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong gpsDataProcessed->setValidity(true, true); } } + return; } else if (validGps) { // Transforming from Degree to Radians and calculation geocentric latitude from geodetic gdLongitude = gpsLongitude * PI / 180.; From 8e2c6a95e05553ddd7888c1d56bfc5d8efb2c6f8 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 8 Aug 2023 15:06:35 +0200 Subject: [PATCH 17/28] debug galore --- mission/controller/acs/util/MathOperations.h | 23 ++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 555f38de..6c5dfa76 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -130,25 +130,44 @@ class MathOperations { double b = 6356752.3142; // semiminor axis [m] // Calculation + sif::debug << "ECEF = [" << vector[0] << "," << vector[1] << "," << vector[2] << "]" + << std::endl; double e2 = 1 - pow(b, 2) / pow(a, 2); + sif::debug << "e2 = " << e2 << std::endl; double epsilon2 = pow(a, 2) / pow(b, 2) - 1; + sif::debug << "epsilon2 = " << epsilon2 << std::endl; double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); + sif::debug << "rho = " << rho << std::endl; double p = std::abs(vector[2]) / epsilon2; + sif::debug << "p = " << p << std::endl; double s = pow(rho, 2) / (e2 * epsilon2); + sif::debug << "s = " << s << std::endl; double q = pow(p, 2) - pow(b, 2) + s; + sif::debug << "q = " << q << std::endl; double u = p / sqrt(q); + sif::debug << "u = " << u << std::endl; double v = pow(b, 2) * pow(u, 2) / q; + sif::debug << "v = " << v << std::endl; double P = 27 * v * s / q; + sif::debug << "P = " << P << std::endl; double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); + sif::debug << "Q = " << Q << std::endl; double t = (1 + Q + 1 / Q) / 6; + sif::debug << "t = " << t << std::endl; double c = sqrt(pow(u, 2) - 1 + 2 * t); + sif::debug << "c = " << c << std::endl; 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)); + sif::debug << "w = " << w << std::endl; + double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4)); + sif::debug << "d = " << d << std::endl; double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); + sif::debug << "N = " << N << std::endl; latitude = asin((epsilon2 + 1) * d / N); + sif::debug << "latitude = " << latitude << std::endl; altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; + sif::debug << "altitude = " << altitude << std::endl; longitude = atan2(vector[1], vector[0]); + sif::debug << "longitude = " << longitude << std::endl; } static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { From 38a8327be01a190d62a6f1f94206ec5c017d1799 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 10:13:19 +0200 Subject: [PATCH 18/28] i hate it here --- mission/controller/acs/util/MathOperations.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 6c5dfa76..de202bf0 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -11,6 +11,8 @@ #include #include +#include "fsfw/serviceinterface.h" + template class MathOperations { public: @@ -158,7 +160,8 @@ class MathOperations { sif::debug << "c = " << c << std::endl; double w = (c - u) / 2; sif::debug << "w = " << w << std::endl; - double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1 / 4)); + double d = + sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); sif::debug << "d = " << d << std::endl; double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); sif::debug << "N = " << N << std::endl; From 1069572392118e6670b5861e2bcd39ae55a1cfb2 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 10:46:24 +0200 Subject: [PATCH 19/28] removed debug output --- mission/controller/acs/util/MathOperations.h | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index de202bf0..dd4c3f57 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -132,45 +132,25 @@ class MathOperations { double b = 6356752.3142; // semiminor axis [m] // Calculation - sif::debug << "ECEF = [" << vector[0] << "," << vector[1] << "," << vector[2] << "]" - << std::endl; double e2 = 1 - pow(b, 2) / pow(a, 2); - sif::debug << "e2 = " << e2 << std::endl; double epsilon2 = pow(a, 2) / pow(b, 2) - 1; - sif::debug << "epsilon2 = " << epsilon2 << std::endl; double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); - sif::debug << "rho = " << rho << std::endl; double p = std::abs(vector[2]) / epsilon2; - sif::debug << "p = " << p << std::endl; double s = pow(rho, 2) / (e2 * epsilon2); - sif::debug << "s = " << s << std::endl; double q = pow(p, 2) - pow(b, 2) + s; - sif::debug << "q = " << q << std::endl; double u = p / sqrt(q); - sif::debug << "u = " << u << std::endl; double v = pow(b, 2) * pow(u, 2) / q; - sif::debug << "v = " << v << std::endl; double P = 27 * v * s / q; - sif::debug << "P = " << P << std::endl; double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); - sif::debug << "Q = " << Q << std::endl; double t = (1 + Q + 1 / Q) / 6; - sif::debug << "t = " << t << std::endl; double c = sqrt(pow(u, 2) - 1 + 2 * t); - sif::debug << "c = " << c << std::endl; double w = (c - u) / 2; - sif::debug << "w = " << w << std::endl; double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); - sif::debug << "d = " << d << std::endl; double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); - sif::debug << "N = " << N << std::endl; latitude = asin((epsilon2 + 1) * d / N); - sif::debug << "latitude = " << latitude << std::endl; altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; - sif::debug << "altitude = " << altitude << std::endl; longitude = atan2(vector[1], vector[0]); - sif::debug << "longitude = " << longitude << std::endl; } static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { From a9fe166b3298d99837ff3238bf9a11a8861d20af Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 10:46:43 +0200 Subject: [PATCH 20/28] do not use raw gps data anywhere --- mission/controller/acs/SensorProcessing.cpp | 13 +++++-------- mission/controller/acs/SensorProcessing.h | 4 ++-- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c93c6027..5a35e8c1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -24,19 +24,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) { { @@ -627,11 +628,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 7bd88fa2..b6b0be0c 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -31,8 +31,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, From f3e18f13131c4eadee0bbce89dced6857c4c6358 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 11:04:59 +0200 Subject: [PATCH 21/28] changelog --- CHANGELOG.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f1f275c..65e197a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,14 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- `eive-tmtc`: + +## Added + +- SPG4 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. + ## Fixed - The handling function of the GPS data is only called once per GPS read. This should remove the fake fix-has-changed events. From d9879013e6500b70cfb5ea24a33493bac192d633 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 11:44:17 +0200 Subject: [PATCH 22/28] fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index d575da85..796c7a9e 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d575da85407e029dabecaffa5368f0c9f1034941 +Subproject commit 796c7a9e377fa197e7e79b9a757d1b8e97419b8f From e2cce1cb51bb98befaf2835840a96ddf9c8f9d77 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 13:41:25 +0200 Subject: [PATCH 23/28] added event for TLE being too old --- mission/acs/defs.h | 2 ++ mission/controller/AcsController.cpp | 36 ++++++++++++++++++++------- mission/controller/AcsController.h | 2 ++ mission/controller/acs/Navigation.cpp | 3 ++- mission/controller/acs/Navigation.h | 2 +- 5 files changed, 34 insertions(+), 11 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 3f0b31c4..827047dd 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -71,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 SPG4 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 6ae108c5..febe385a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -167,13 +167,19 @@ void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); - navigation.useSpg4(now, &gpsDataProcessed); + 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) { @@ -293,13 +299,19 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); - navigation.useSpg4(now, &gpsDataProcessed); + 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) { @@ -380,11 +392,17 @@ void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); - navigation.useSpg4(now, &gpsDataProcessed); + 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++; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a40dba3b..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; diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 61f9bda4..4726586b 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -45,7 +45,7 @@ void Navigation::resetMekf(acsctrl::MekfData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } -void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) { +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); @@ -75,6 +75,7 @@ void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcesse } } } + return result; } ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) { diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index bc39555f..2785ff2e 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -21,7 +21,7 @@ class Navigation { AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); - void useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); + ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); protected: From b4c3553965a03dafd176fec91844c99796930b6f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 13:41:41 +0200 Subject: [PATCH 24/28] changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 65e197a0..2fdbf546 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,7 +22,8 @@ will consitute of a breaking change warranting a new major release: - SPG4 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. + 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 SPG4 solution. ## Fixed - The handling function of the GPS data is only called once per GPS read. This should remove From 653d13960fb98b41325bd018922bfc669c153376 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 13:42:54 +0200 Subject: [PATCH 25/28] ability to disable usage of sgp4 --- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/SensorProcessing.cpp | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) 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/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 5a35e8c1..5b47c9c3 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -541,7 +541,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong gpsVelocityE[3] = {0, 0, 0}; 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) { + 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); From 920109564475e23dad4bb3f33c9d0b03e7e9fc90 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 9 Aug 2023 13:44:10 +0200 Subject: [PATCH 26/28] typos --- CHANGELOG.md | 4 ++-- mission/acs/defs.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2fdbf546..9946093e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,10 +20,10 @@ will consitute of a breaking change warranting a new major release: ## Added -- SPG4 Propagator is now used for propagating the position of EIVE. It will only work once +- 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 SPG4 solution. + according parameter, the ACS Controller can be directed to ignore the SGP4 solution. ## Fixed - The handling function of the GPS data is only called once per GPS read. This should remove diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 827047dd..8cdd283b 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -71,7 +71,7 @@ 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 SPG4 Propagator has become too old. +//! [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); From 15421240f1ac2b8878eaedbc01d77b2804cc789b Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 12:48:22 +0200 Subject: [PATCH 27/28] naming --- mission/acs/defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 8cdd283b..baed276d 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -45,7 +45,7 @@ enum SafeModeStrategy : uint8_t { enum GpsSource : uint8_t { NONE, GPS, - PROPAGATION, + GPS_EXTRAPOLATED, SPG4, }; From 93896557aabfa86fc9e282762a88f2c4523430cc Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 14 Aug 2023 15:36:20 +0200 Subject: [PATCH 28/28] fuck math.h --- mission/controller/acs/SensorProcessing.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 8805f233..a683aada 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -187,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); @@ -546,8 +546,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong return; } else if (validGps) { // Transforming from Degree to Radians and calculation geocentric latitude from geodetic - gdLongitude = gpsLongitude * PI / 180.; - double latitudeRad = gpsLatitude * PI / 180.; + gdLongitude = gpsLongitude * M_PI / 180.; + double latitudeRad = gpsLatitude * M_PI / 180.; double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(latitudeRad));