mission/controller/AcsController.cpp

@@ -22,8 +22,7 @@ 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();
@@ -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;
       }
-      {
+      result = writeTleToFs(data);
-        PoolReadGuard pg(&tleData);
+      if (result != returnvalue::OK) {
-        if (pg.getReadResult() == returnvalue::OK) {
+        return result;
-          std::memcpy(tleData.line1.value, data, 69);
-          std::memcpy(tleData.line2.value, data + 69, 69);
-          tleData.setValidity(true, true);
-        }
       }
       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);

mission/controller/AcsController.h

@@ -25,6 +25,8 @@
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 #include <mission/utility/trace.h>
 
+#include <fstream>
+
 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<double> rotRateParallel = PoolEntry<double>(3);
   PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
 
-  // TLE Dataset
-  acsctrl::TleData tleData;
-  PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
-  PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
-
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
 };

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<FUSED_ROT_RATE_SET_ENTRIES> {
  private:
 };
 
-class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
- public:
-  TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
-
-  lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
-  lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
-
- private:
-};
-
 }  // namespace acsctrl
 
 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */