eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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 && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO); triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true; mekfLost = true;
} }
} else { } else {
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
// get desired satellite rate and sun direction to align // get desired satellite rate, sun direction to align to and inertia
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0},
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// if MEKF is working guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { ReturnValue_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
mgmDataProcessed.magIgrfModel.value, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid());
mgmDataProcessed.magIgrfModel.isValid(), switch (safeCtrlStrat) {
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), case (SafeCtrl::SAFECTRL_USE_MEKF):
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
sunTargetDir, satRateSafe, &errAng, magMomMtq); susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
} else { satRateSafe, magMomMtq, errAng);
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) {
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
} break;
} else { case (SafeCtrl::SAFECTRL_USE_NONMEKF):
safeCtrlFailureFlag = false; safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
safeCtrlFailureCounter = 0; 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, actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
@ -237,11 +256,13 @@ void AcsController::performDetumble() {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO); triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true;
} }
} else { } else {
mekfInvalidFlag = false; mekfInvalidFlag = false;
@ -291,11 +312,13 @@ void AcsController::performPointingCtrl() {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO); triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true;
} }
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty. // Trigger this so STR FDIR can set the device faulty.