eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Store TLE presistent #789

Merged
meggert merged 17 commits from persistent-tle-store into dev-7.5.0 2023-12-01 13:04:57 +01:00
Conversation 0 Commits 17 Files Changed 10 +71 -34
3 changed files with 71 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8e41885ca1 - Show all commits

76
mission/controller/AcsController.cpp
View File

@ -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);

15
mission/controller/AcsController.h
View File

@ -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);
};

14
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -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_ */