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

Merge branch 'dev-7.5.0' into higher-acs-modes-only-str
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-12-04 13:15:25 +01:00
parent 08d0619b11 64069aa5c1
commit d0588b144a
64 changed files with 2779 additions and 629 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
mission/controller/AcsController.cpp
View File

@ -1,12 +1,9 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/acs/defs.h>
#include <mission/config/torquer.h>
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
sdcMan(sdcMan),
attitudeEstimation(&acsParameters),
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
@ -24,8 +21,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this),
fusedRotRateSourcesData(this),
tleData(this) {}
fusedRotRateSourcesData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -69,22 +65,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;
}
@ -132,6 +133,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;
@ -816,9 +821,6 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
// TLE Data
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
return returnvalue::OK;
}
@ -1046,6 +1048,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<const char *>(tle), 69);
tleFile << "\n";
tleFile.write(reinterpret_cast<const char *>(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);

28
mission/controller/AcsController.h
View File

@ -4,15 +4,18 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <mission/acs/defs.h>
#include <mission/acs/imtqHelpers.h>
#include <mission/acs/rwHelpers.h>
#include <mission/acs/susMax1227Helpers.h>
#include <mission/config/torquer.h>
#include <mission/controller/acs/ActuatorCmd.h>
#include <mission/controller/acs/AttitudeEstimation.h>
#include <mission/controller/acs/FusedRotationEstimation.h>
@ -24,13 +27,18 @@
#include <mission/controller/acs/control/PtgCtrl.h>
#include <mission/controller/acs/control/SafeCtrl.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <mission/memory/SdCardMountedIF.h>
#include <mission/utility/trace.h>
#include <filesystem>
#include <fstream>
#include <optional>
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,
@ -51,6 +59,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool enableHkSets = false;
SdCardMountedIF& sdcMan;
timeval timeAbsolute;
timeval timeRelative;
double timeDelta = 0.0;
@ -95,10 +105,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;
@ -133,6 +148,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;
@ -256,11 +277,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalStr = 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

@ -119,9 +119,6 @@ enum PoolIds : lp_id_t {
ROT_RATE_TOTAL_SUSMGM,
ROT_RATE_TOTAL_QUEST,
ROT_RATE_TOTAL_STR,
// TLE
TLE_LINE_1,
TLE_LINE_2,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -136,7 +133,6 @@ 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 = 4;
static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
static constexpr uint8_t TLE_SET_ENTRIES = 2;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -329,16 +325,6 @@ class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENT
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_ */