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

this mekf should work
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2024-02-27 16:05:05 +01:00
parent f382bd3e61
commit 352053f1d2
6 changed files with 757 additions and 1214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,40 +1,27 @@
#include "Navigation.h"
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
Navigation::Navigation() {}
Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
Navigation::~Navigation() {}
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::AttitudeEstimationData *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;
ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
const acsctrl::GyrDataProcessed *gyrDataProcessed,
const acsctrl::MgmDataProcessed *mgmDataProcessed,
const acsctrl::SusDataProcessed *susDataProcessed,
const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
multiplicativeKalmanFilter.setStrData(
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
sensorValues->strSet.caliQx.isValid());
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);
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
gyrDataProcessed, attitudeEstimationData);
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);
susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
return mekfStatus;
}
}
@ -79,3 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
return sgp4Propagator.initialize(line1, line2);
}
void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
}