diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index d90f3f51..57526333 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1088,13 +1088,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c return KALMAN_RUNNING; } -void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { double resetQuaternion[4] = {0, 0, 0, 1}; double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); + return KALMAN_UNINITIALIZED; } void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index dd02cf9f..d8bcd105 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -29,7 +29,7 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(AcsParameters *acsParameters_); virtual ~MultiplicativeKalmanFilter(); - void reset(acsctrl::MekfData *mekfData); + ReturnValue_t reset(acsctrl::MekfData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index c6310302..da4a21a8 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -25,26 +25,25 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); - if (kalmanInit) { - return multiplicativeKalmanFilter.mekfEst( + if (mekfStatus == MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED) { + mekfStatus = multiplicativeKalmanFilter.init( + mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), + susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), + susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); + return mekfStatus; + } else { + mekfStatus = multiplicativeKalmanFilter.mekfEst( quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); - } else { - ReturnValue_t result; - result = multiplicativeKalmanFilter.init( - mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), - susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), - susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); - kalmanInit = true; - return result; + return mekfStatus; } } void Navigation::resetMekf(acsctrl::MekfData *mekfData) { - multiplicativeKalmanFilter.reset(mekfData); + mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index e054bfbf..86baeacf 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -22,7 +22,7 @@ class Navigation { private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; AcsParameters acsParameters; - bool kalmanInit = false; + ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED; }; #endif /* ACS_NAVIGATION_H_ */