diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 65515b78..c6448a82 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -124,17 +124,25 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + mekfInvalidFlag = true; + } + return; + } 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 double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; - if (validMekf == returnvalue::OK) { + if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), @@ -183,10 +191,18 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + mekfInvalidFlag = true; + } + return; + } else { + mekfInvalidFlag = false; + } double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, @@ -224,13 +240,21 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_PTG_MODE); + mekfInvalidFlag = true; + } + return; + } else { + mekfInvalidFlag = false; + } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { multipleRwUnavailableCounter++; if (multipleRwUnavailableCounter > 4) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 715baef4..9e6c2c4e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -10,6 +10,7 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" +#include "acs/MultiplicativeKalmanFilter.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" @@ -54,6 +55,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; + bool mekfInvalidFlag = true; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif