eive-obsw/mission/controller/acs/Navigation.cpp
Marius Eggert e140ae4063
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
fixed typo in filename
2022-09-26 10:13:00 +02:00

50 lines
1.7 KiB
C++

/*
* Navigation.cpp
*
* Created on: 23 May 2022
* Author: Robin Marquardt
*/
#include "Navigation.h"
#include <math.h>
#include <acs/util/MathOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <acs/util/CholeskyDecomposition.h>
Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){
acsParameters = *acsParameters_;
}
Navigation::~Navigation(){
}
void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){
if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid,
outputValues->satRateEst, &outputValues->satRateEstValid,
outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid,
outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
}
else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid);
kalmanInit = true;
*mekfValid = 0;
// 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 ?
}
}