eive-obsw/mission/controller/acs/Navigation.cpp

58 lines
2.5 KiB
C++
Raw Normal View History

2022-09-20 14:15:55 +02:00
/*
* Navigation.cpp
*
* Created on: 23 May 2022
* Author: Robin Marquardt
*/
#include "Navigation.h"
2022-09-27 11:06:11 +02:00
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
2022-09-27 11:06:11 +02:00
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
2022-09-20 14:15:55 +02:00
Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
acsParameters = *acsParameters_;
2022-09-20 14:15:55 +02:00
}
Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
ReturnValue_t *mekfValid) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst(
quatJB, quatJBValid, 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); // VALIDS FOR QUAT AND RATE ??
} else {
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());
kalmanInit = true;
*mekfValid = returnvalue::OK;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the
// init of kalman filter where does this class know from that kalman filter was not
// initialized ?
}
2022-09-20 14:15:55 +02:00
}