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: