From 8e41885ca182bcddcef7747556221195e6e175a5 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 23 Aug 2023 15:21:40 +0200 Subject: [PATCH] tle gets stored presistent now --- mission/controller/AcsController.cpp | 76 +++++++++++++++---- mission/controller/AcsController.h | 15 ++-- .../AcsCtrlDefinitions.h | 14 ---- 3 files changed, 71 insertions(+), 34 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..f0c1f3e0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -22,8 +22,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,19 +66,13 @@ 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; } @@ -130,6 +123,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 +798,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 +1025,58 @@ 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) { + // split TLE to two lines + // not sure why these need to be 70 long + uint8_t tleLine1[70] = {}; + uint8_t tleLine2[70] = {}; + std::memcpy(tleLine1, tle, 69); + std::memcpy(tleLine2, tle + 69, 69); + // ToDo: check which SD card is active via sdcMan + std::string path = SD_0 + TLE_FILE; + // Clear existing TLE from file + std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); + if (tleFile.is_open()) { + tleFile << tleLine1 << "\n" << tleLine2 << "\n"; + } else { + // return error + } + tleFile.close(); + return returnvalue::OK; +} + +ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { + // ToDo: check which SD card is active via sdcMan + std::string path = SD_0 + TLE_FILE; + // 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 { + // return error + } + tleFile.close(); + return returnvalue::OK; +} + void AcsController::copyGyrData() { { PoolReadGuard pg(&sensorValues.gyr0AdisSet); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..9d91ae10 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -25,6 +25,8 @@ #include #include +#include + class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; @@ -125,6 +127,14 @@ 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 SD_0 = "/mnt/sd0/"; + const std::string SD_1 = "/mnt/sd1/"; + const std::string TLE_FILE = "conf/tle.txt"; + /* ACS Sensor Values */ ACS::SensorValues sensorValues; @@ -238,11 +248,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_ */