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

use safeCtrl strategy

Browse Source
This commit is contained in:
Marius Eggert 2023-03-24 14:52:12 +01:00
parent 906413e800
commit dce6323090
1 changed files with 57 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
mission/controller/AcsController.cpp
View File

@ -150,50 +150,69 @@ void AcsController::performSafe() {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else {
mekfInvalidFlag = false;
}
// get desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// if MEKF is working
// get desired satellite rate, sun direction to align to and inertia
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0},
inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value,
mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq);
} else {
result = safeCtrl.safeNoMekf(
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq);
}
if (result == returnvalue::FAILED) {
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE);
safeCtrlFailureFlag = true;
}
safeCtrlFailureCounter++;
if (safeCtrlFailureCounter > 150) {
ReturnValue_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid());
switch (safeCtrlStrat) {
case (SafeCtrl::SAFECTRL_USE_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
satRateSafe, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
}
} else {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_USE_NONMEKF):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_USE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
satRateSafe, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (SafeCtrl::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 1, 0);
safeCtrlFailureFlag = true;
}
safeCtrlFailureCounter++;
if (safeCtrlFailureCounter > 150) {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
}
break;
case (SafeCtrl::SAFECTRL_NO_SENSORS_FOR_CONTROL):
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 0, 1);
safeCtrlFailureFlag = true;
}
safeCtrlFailureCounter++;
if (safeCtrlFailureCounter > 150) {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
}
break;
}
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
@ -237,11 +256,13 @@ void AcsController::performDetumble() {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else {
mekfInvalidFlag = false;
@ -291,11 +312,13 @@ void AcsController::performPointingCtrl() {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.