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

removed OutputValues

Browse Source 
amended sumbode list
inserted writes to output DataPools
This commit is contained in:
Marius Eggert
2022-11-03 10:43:27 +01:00
parent 44dda9455d
commit a13ccb43d2
24 changed files with 590 additions and 522 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,28 +21,34 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
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());
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 ??
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(),
mekfData); // VALIDS FOR QUAT AND RATE ??
} else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid);
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 = 0;
*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