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

throw a single event every time the mekf stops working
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-21 17:10:59 +01:00
parent 019cc29c24
commit f19729e11d
2 changed files with 41 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
mission/controller/AcsController.cpp
View File

@ -124,17 +124,25 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
mekfInvalidFlag = true;
}
return;
} else {
mekfInvalidFlag = false;
}
// get desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
@ -183,10 +191,18 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
mekfInvalidFlag = true;
}
return;
} else {
mekfInvalidFlag = false;
}
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
@ -224,13 +240,21 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_PTG_MODE);
mekfInvalidFlag = true;
}
return;
} else {
mekfInvalidFlag = false;
}
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
multipleRwUnavailableCounter++;
if (multipleRwUnavailableCounter > 4) {

3
mission/controller/AcsController.h
View File

@ -10,6 +10,7 @@
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
#include "acs/MultiplicativeKalmanFilter.h"
#include "acs/Navigation.h"
#include "acs/SensorProcessing.h"
#include "acs/control/Detumble.h"
@ -54,6 +55,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ParameterHelper parameterHelper;
bool mekfInvalidFlag = true;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif