ACS Ptg Ctrl Fixes #643
@ -169,7 +169,7 @@ void AcsController::performSafe() {
|
||||
guidance.getTargetParamsSafe(sunTargetDir);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
||||
acsParameters.safeModeControllerParameters.useMekf,
|
||||
@ -205,6 +205,9 @@ void AcsController::performSafe() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
@ -259,7 +262,7 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::MEKF_RECOVERY);
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
uint8_t safeCtrlStrat = detumble.detumbleStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
||||
@ -279,6 +282,9 @@ void AcsController::performDetumble() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
@ -477,7 +483,7 @@ void AcsController::performPointingCtrl() {
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl";
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -7,8 +7,10 @@ Detumble::Detumble() {}
|
||||
|
||||
Detumble::~Detumble() {}
|
||||
|
||||
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw) {
|
||||
acs::SafeModeStrategy 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;
|
||||
} else if (satRotRateValid and useFullDetumbleLaw) {
|
||||
|
@ -11,8 +11,9 @@ class Detumble {
|
||||
Detumble();
|
||||
virtual ~Detumble();
|
||||
|
||||
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw);
|
||||
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw);
|
||||
|
||||
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
|
||||
double gain);
|
||||
|
@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
|
||||
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled) {
|
||||
const uint8_t mekfEnabled,
|
||||
const uint8_t dampingEnabled) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
|
@ -12,7 +12,7 @@ class SafeCtrl {
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
acs::SafeModeStrategy 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