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

reseting the MEKF now also resets its state in navigation
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-23 09:45:11 +01:00
parent 29f1dd0f8e
commit 205bd4648e
4 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
mission/controller/acs/Navigation.cpp
View File

@ -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);
}