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

useMekf actually now uses returnvalues as inteded

Browse Source
This commit is contained in:
Marius Eggert 2023-02-21 17:09:49 +01:00
parent d1402587dc
commit 019cc29c24
2 changed files with 23 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,37 +14,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
Navigation::~Navigation() {} Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed,
ReturnValue_t *mekfValid) { acsctrl::MekfData *mekfData) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid = sensorValues->strSet.caliQx.isValid() && bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) { if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst( return multiplicativeKalmanFilter.mekfEst(
quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value, quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init( ReturnValue_t result;
result = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
kalmanInit = true; kalmanInit = true;
*mekfValid = returnvalue::OK; return result;
// 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::resetMekf(acsctrl::MekfData *mekfData) {
multiplicativeKalmanFilter.reset(mekfData);
}

19
mission/controller/acs/Navigation.h
View File

@ -1,10 +1,3 @@
/*
* Navigation.h
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#ifndef NAVIGATION_H_ #ifndef NAVIGATION_H_
#define NAVIGATION_H_ #define NAVIGATION_H_
@ -16,14 +9,14 @@
class Navigation { class Navigation {
public: public:
Navigation(AcsParameters *acsParameters_); // Input mode ? Navigation(AcsParameters *acsParameters_);
virtual ~Navigation(); virtual ~Navigation();
void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, acsctrl::MgmDataProcessed *mgmDataProcessed,
ReturnValue_t *mekfValid); acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
void processSensorData(); void resetMekf(acsctrl::MekfData *mekfData);
protected: protected:
private: private: