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

use enum instead of rtval to enable easier tm generation
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-05 17:20:29 +02:00
parent 39b2a3420c
commit 65dd0f313b
5 changed files with 33 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
mission/controller/AcsController.cpp
View File

@ -170,31 +170,31 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
ReturnValue_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid());
switch (safeCtrlStrat) {
case (SafeCtrl::SAFECTRL_USE_MEKF):
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
satRateSafe, inertiaEive, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_USE_NONMEKF):
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_NONMEKF):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive,
magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_USE_DAMPING):
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
satRateSafe, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 1, 0);
safeCtrlFailureFlag = true;
@ -205,7 +205,7 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0;
}
break;
case (SafeCtrl::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 0, 1);
safeCtrlFailureFlag = true;
@ -241,7 +241,7 @@ void AcsController::performSafe() {
startTransition(mode, acs::SafeSubmode::DETUMBLE);
}
updateCtrlValData(errAng);
updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
@ -545,7 +545,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
}
}
void AcsController::updateCtrlValData(double errAng) {
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
@ -556,6 +556,8 @@ void AcsController::updateCtrlValData(double errAng) {
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
@ -568,6 +570,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = SafeCtrl::SafeModeStrategy::SAFECTRL_OFF;
ctrlValData.setValidity(true, true);
}
}

2
mission/controller/AcsController.h
View File

@ -111,7 +111,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(double errAng);
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate);
void disableCtrlValData();

14
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -13,18 +13,18 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid) {
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid) {
if (not magFieldValid) {
return SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfValid) {
return SAFECTRL_USE_MEKF;
return SafeModeStrategy::SAFECTRL_USE_MEKF;
} else if (satRotRateValid and sunDirValid) {
return SAFECTRL_USE_NONMEKF;
return SafeModeStrategy::SAFECTRL_USE_NONMEKF;
} else if (satRotRateValid and not sunDirValid) {
return SAFECTRL_USE_DAMPING;
return SafeModeStrategy::SAFECTRL_USE_DAMPING;
} else {
return SAFECTRL_NO_SENSORS_FOR_CONTROL;
return SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
}

13
mission/controller/acs/control/SafeCtrl.h
View File

@ -22,8 +22,17 @@ class SafeCtrl {
static constexpr ReturnValue_t SAFECTRL_NO_SENSORS_FOR_CONTROL =
returnvalue::makeCode(IF_SAFE_ID, 6);
ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid);
enum SafeModeStrategy : uint8_t {
SAFECTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
SAFECTRL_USE_MEKF = 10,
SAFECTRL_USE_NONMEKF = 11,
SAFECTRL_USE_DAMPING = 12,
};
uint8_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, const double satRotRateRef,

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

@ -94,6 +94,7 @@ enum PoolIds : lp_id_t {
QUAT_MEKF,
MEKF_STATUS,
// Ctrl Values
SAFE_STRAT,
TGT_QUAT,
ERROR_QUAT,
ERROR_ANG,
@ -112,7 +113,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/**
@ -251,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
public:
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
lp_var_t<uint8_t> safeStrat = lp_var_t<uint8_t>(sid.objectId, SAFE_STRAT, this);
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);