mission/controller
@ -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);
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user