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

added PtgCtrl code. matched inputs to actual SensorValues inputs
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2022-10-12 10:28:44 +02:00
parent 7be6bbc948
commit 4aecd07970
3 changed files with 75 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,45 +6,46 @@
*/
#include "Navigation.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <math.h>
#include <fsfw/globalfunctions/math/VectorOperations.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(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){
acsParameters = *acsParameters_;
Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
acsParameters = *acsParameters_;
}
Navigation::~Navigation(){
Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
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, 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 ?
}
}
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 ?
}
}