47 lines
2.1 KiB
C++
47 lines
2.1 KiB
C++
#include "Navigation.h"
|
|
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
#include "util/CholeskyDecomposition.h"
|
|
#include "util/MathOperations.h"
|
|
|
|
Navigation::Navigation() {}
|
|
|
|
Navigation::~Navigation() {}
|
|
|
|
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
|
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
|
acsctrl::SusDataProcessed *susDataProcessed,
|
|
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
|
|
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
|
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
|
|
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
|
|
|
|
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_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,
|
|
acsParameters);
|
|
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(), mekfData, acsParameters);
|
|
return mekfStatus;
|
|
}
|
|
}
|
|
|
|
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
|
|
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
|
|
}
|