improved naming
All checks were successful
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev This commit looks good
This commit is contained in:
parent
14ac05137c
commit
69be05e621
|
@ -22,7 +22,7 @@ enum AcsMode : Mode_t {
|
|||
|
||||
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
|
||||
|
||||
enum SafeModeStrategy : uint8_t {
|
||||
enum ctrlStrategy : uint8_t {
|
||||
SAFECTRL_OFF = 0,
|
||||
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
|
||||
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
|
||||
|
@ -32,6 +32,8 @@ enum SafeModeStrategy : uint8_t {
|
|||
SAFECTRL_ECLIPSE_IDELING = 13,
|
||||
SAFECTRL_DETUMBLE_FULL = 20,
|
||||
SAFECTRL_DETUMBLE_DETERIORATED = 21,
|
||||
PTGCTRL_ACTIVE_MEKF = 30,
|
||||
PTGCTRL_WITHOUT_MEKF = 31,
|
||||
};
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
||||
|
|
|
@ -169,40 +169,40 @@ void AcsController::performSafe() {
|
|||
guidance.getTargetParamsSafe(sunTargetDir);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
acs::ctrlStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
||||
acsParameters.safeModeControllerParameters.useMekf,
|
||||
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
|
||||
switch (safeCtrlStrat) {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
|
||||
case (acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF):
|
||||
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
|
||||
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
|
||||
magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
|
||||
case (acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF):
|
||||
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
||||
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
|
||||
case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING):
|
||||
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
||||
sunTargetDir, magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
|
||||
case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING):
|
||||
errAng = NAN;
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
|
@ -261,24 +261,24 @@ void AcsController::performDetumble() {
|
|||
triggerEvent(acs::MEKF_RECOVERY);
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
acs::ctrlStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
switch (safeCtrlStrat) {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
|
||||
case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL):
|
||||
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainFull);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
||||
case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainBdot);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
|
@ -602,7 +602,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 = acs::SafeModeStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.safeStrat.value = acs::ctrlStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -7,18 +7,18 @@ Detumble::Detumble() {}
|
|||
|
||||
Detumble::~Detumble() {}
|
||||
|
||||
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
|
||||
acs::ctrlStrategy Detumble::detumbleStrategy(const bool magFieldValid,
|
||||
const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (satRotRateValid and useFullDetumbleLaw) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
|
||||
return acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL;
|
||||
} else if (magFieldRateValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
|
||||
return acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
|
||||
} else {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
||||
return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -11,7 +11,7 @@ class Detumble {
|
|||
Detumble();
|
||||
virtual ~Detumble();
|
||||
|
||||
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
acs::ctrlStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw);
|
||||
|
||||
|
|
|
@ -9,22 +9,22 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
|
|||
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
acs::ctrlStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled,
|
||||
const uint8_t dampingEnabled) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF;
|
||||
return acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF;
|
||||
} else if (satRotRateValid and sunDirValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
|
||||
return acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF;
|
||||
} else if (dampingEnabled and satRotRateValid and not sunDirValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
|
||||
return acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING;
|
||||
} else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
|
||||
return acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING;
|
||||
} else {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
||||
return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -12,7 +12,7 @@ class SafeCtrl {
|
|||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
acs::ctrlStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
|
||||
|
||||
|
|
Loading…
Reference in New Issue
Block a user