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

Parameter for STR for MEKF #877

Merged
meggert merged 2 commits from no-str-for-mekf into main 2024-03-18 10:58:54 +01:00
Conversation 0 Commits 2 Files Changed 8 +16 -8
7 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c4680f85bb - Show all commits

3
mission/controller/AcsController.cpp
View File

@ -180,7 +180,8 @@ void AcsController::performAttitudeControl() {
mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, timeDelta, &attitudeEstimationData); &susDataProcessed, timeDelta, &attitudeEstimationData,
acsParameters.kalmanFilterParameters.allowStr);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {

3
mission/controller/acs/AcsParameters.cpp
View File

@ -734,6 +734,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5: case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs); parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
break; break;
case 0x6:
parameterWrapper->set(kalmanFilterParameters.allowStr);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }

2
mission/controller/acs/AcsParameters.h
View File

@ -947,6 +947,8 @@ class AcsParameters : public HasParametersIF {
double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
uint8_t allowStr = false;
} kalmanFilterParameters; } kalmanFilterParameters;
struct MagnetorquerParameter { struct MagnetorquerParameter {

6
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -626,11 +626,11 @@ void MultiplicativeKalmanFilter::updateDataSet(
} }
} }
void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, bool valid,
bool valid) { bool allowStr) {
strData.strQuat.value[0] = qX; strData.strQuat.value[0] = qX;
strData.strQuat.value[1] = qY; strData.strQuat.value[1] = qY;
strData.strQuat.value[2] = qZ; strData.strQuat.value[2] = qZ;
strData.strQuat.value[3] = qW; strData.strQuat.value[3] = qW;
strData.strQuat.valid = valid; strData.strQuat.valid = (valid and allowStr);
} }

2
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -38,7 +38,7 @@ class MultiplicativeKalmanFilter {
void updateStandardDeviations(const AcsParameters *acsParameters); void updateStandardDeviations(const AcsParameters *acsParameters);
void setStrData(const double qX, const double qY, const double qZ, const double qW, void setStrData(const double qX, const double qY, const double qZ, const double qW,
const bool valid); const bool valid, const bool allowStr);
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF; static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2); static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);

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

@ -9,11 +9,12 @@ ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
const acsctrl::MgmDataProcessed *mgmDataProcessed, const acsctrl::MgmDataProcessed *mgmDataProcessed,
const acsctrl::SusDataProcessed *susDataProcessed, const acsctrl::SusDataProcessed *susDataProcessed,
const double timeDelta, const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData) { acsctrl::AttitudeEstimationData *attitudeEstimationData,
const bool allowStr) {
multiplicativeKalmanFilter.setStrData( multiplicativeKalmanFilter.setStrData(
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
sensorValues->strSet.caliQx.isValid()); sensorValues->strSet.caliQx.isValid(), allowStr);
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed, mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,

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

@ -17,7 +17,8 @@ class Navigation {
const acsctrl::GyrDataProcessed *gyrDataProcessed, const acsctrl::GyrDataProcessed *gyrDataProcessed,
const acsctrl::MgmDataProcessed *mgmDataProcessed, const acsctrl::MgmDataProcessed *mgmDataProcessed,
const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta, const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
acsctrl::AttitudeEstimationData *attitudeEstimationData); acsctrl::AttitudeEstimationData *attitudeEstimationData,
const bool allowStr);
void resetMekf(acsctrl::AttitudeEstimationData *mekfData); void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);