diff --git a/CHANGELOG.md b/CHANGELOG.md index a7dd8a6f..e0926cc4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,11 @@ will consitute of a breaking change warranting a new major release: ## Changed - ACS-Board default side changed to B-Side +- The TLE uploaded now gets stored in a file on the filesystem. It will always be stored on + the current active SD Card. After a reboot, the TLE will be read from the filesystem. + A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even + though it is there. +- Added action cmd to read the currently stored TLE. # [v7.4.0] 2023-11-30 diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 65456d1e..949f969b 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -175,7 +175,7 @@ void ObjectFactory::produce(void* args) { createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); #endif - createAcsController(true, enableHkSets); + createAcsController(true, enableHkSets, *SdCardManager::instance()); HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); createThermalController(*heaterHandler, true); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 05704abf..71722e52 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -130,6 +130,6 @@ void ObjectFactory::produce(void* args) { createMiscComponents(); createThermalController(*heaterHandler, false); - createAcsController(true, enableHkSets); + createAcsController(true, enableHkSets, *SdCardManager::instance()); satsystem::init(false); } diff --git a/fsfw b/fsfw index b28174db..7187f2b5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit b28174db249cb33b541f665270fd6af14c382351 +Subproject commit 7187f2b5cdfe163bf7ed1a8fab48900d69f4c8bf diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index 756dc0af..4956c1d6 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -335,8 +335,9 @@ void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwr scexHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); } -AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets) { - auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets); +AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets, + SdCardMountedIF& mountedIF) { + auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets, mountedIF); if (connectSubsystem) { acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); } diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index 46baa685..b813d823 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -31,7 +31,8 @@ void createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher, void gpioChecker(ReturnValue_t result, std::string output); -AcsController* createAcsController(bool connectSubsystem, bool enableHkSets); +AcsController* createAcsController(bool connectSubsystem, bool enableHkSets, + SdCardMountedIF& mountedIF); PowerController* createPowerController(bool connectSubsystem, bool enableHkSets); } // namespace ObjectFactory diff --git a/mission/acs/defs.h b/mission/acs/defs.h index baed276d..91030839 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -73,6 +73,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG 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); +//! [EXPORT] : [COMMENT] The TLE could not be read from the filesystem. +static constexpr Event TLE_FILE_READ_FAILED = MAKE_EVENT(9, severity::LOW); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..8bdc7d15 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,12 +1,9 @@ #include "AcsController.h" -#include -#include -#include - -AcsController::AcsController(object_id_t objectId, bool enableHkSets) +AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + sdcMan(sdcMan), fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), @@ -22,8 +19,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) mekfData(this), ctrlValData(this), actuatorCmdData(this), - fusedRotRateData(this), - tleData(this) {} + fusedRotRateData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -67,22 +63,27 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t if (size != 69 * 2) { return INVALID_PARAMETERS; } - ReturnValue_t result = navigation.updateTle(data, data + 69); + ReturnValue_t result = updateTle(data, data + 69, false); 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); - } + result = writeTleToFs(data); + if (result != returnvalue::OK) { + return result; } return HasActionsIF::EXECUTION_FINISHED; } + case (READ_TLE): { + uint8_t tle[69 * 2] = {}; + uint8_t line2[69] = {}; + ReturnValue_t result = readTleFromFs(tle, line2); + if (result != returnvalue::OK) { + return result; + } + std::memcpy(tle + 69, line2, 69); + actionHelper.reportData(commandedBy, actionId, tle, 69 * 2); + return EXECUTION_FINISHED; + } default: { return HasActionsIF::INVALID_ACTION_ID; } @@ -130,6 +131,10 @@ void AcsController::performControlOperation() { } case InternalState::INITIAL_DELAY: { if (initialCountdown.hasTimedOut()) { + uint8_t line1[69] = {}; + uint8_t line2[69] = {}; + readTleFromFs(line1, line2); + updateTle(line1, line2, true); internalState = InternalState::READY; } return; @@ -801,9 +806,6 @@ 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; } @@ -1031,6 +1033,67 @@ void AcsController::copySusData() { } } +ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) { + ReturnValue_t result = navigation.updateTle(line1, line2); + if (result != returnvalue::OK) { + if (not fromFile) { + uint8_t fileLine1[69] = {}; + uint8_t fileLine2[69] = {}; + readTleFromFs(fileLine1, fileLine2); + navigation.updateTle(fileLine1, fileLine2); + } + return result; + } + return returnvalue::OK; +} + +ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { + auto mntPrefix = sdcMan.getCurrentMountPrefix(); + if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { + return returnvalue::FAILED; + } + std::string path = mntPrefix + TLE_FILE; + // Clear existing TLE from file + std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); + if (tleFile.is_open()) { + tleFile.write(reinterpret_cast(tle), 69); + tleFile << "\n"; + tleFile.write(reinterpret_cast(tle + 69), 69); + } else { + return WRITE_FILE_FAILED; + } + tleFile.close(); + return returnvalue::OK; +} + +ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { + auto mntPrefix = sdcMan.getCurrentMountPrefix(); + if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { + return returnvalue::FAILED; + } + std::string path = mntPrefix + TLE_FILE; + std::error_code e; + if (std::filesystem::exists(path, e)) { + // Read existing TLE from file + std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in); + if (tleFile.is_open()) { + std::string tleLine1, tleLine2; + getline(tleFile, tleLine1); + std::memcpy(line1, tleLine1.c_str(), 69); + getline(tleFile, tleLine2); + std::memcpy(line2, tleLine2.c_str(), 69); + } else { + triggerEvent(acs::TLE_FILE_READ_FAILED); + return READ_FILE_FAILED; + } + tleFile.close(); + } else { + triggerEvent(acs::TLE_FILE_READ_FAILED); + return READ_FILE_FAILED; + } + return returnvalue::OK; +} + void AcsController::copyGyrData() { { PoolReadGuard pg(&sensorValues.gyr0AdisSet); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..4d80da3e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -4,15 +4,18 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include #include #include +#include #include #include #include @@ -23,13 +26,18 @@ #include #include #include +#include #include +#include +#include +#include + class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; - AcsController(object_id_t objectId, bool enableHkSets); + AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF& sdcMan); MessageQueueId_t getCommandQueue() const; ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, @@ -49,6 +57,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool enableHkSets = false; + SdCardMountedIF& sdcMan; + AcsParameters acsParameters; SensorProcessing sensorProcessing; FusedRotationEstimation fusedRotationEstimation; @@ -87,10 +97,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes 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 DeviceCommandId_t READ_TLE = 0x4; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent. static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); + //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. + static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1); + //! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. + static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2); ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; @@ -125,6 +140,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, const double* tgtRotRate); + ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile); + ReturnValue_t writeTleToFs(const uint8_t* tle); + ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2); + + const std::string TLE_FILE = "/conf/tle.txt"; + /* ACS Sensor Values */ ACS::SensorValues sensorValues; @@ -238,11 +259,6 @@ 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/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index b843ca0c..ac348cf9 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -110,9 +110,6 @@ 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; @@ -126,7 +123,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. @@ -299,16 +295,6 @@ 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_ */