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

fdir needs to change
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-11-14 16:57:55 +01:00
parent 5512605cd7
commit 9482f3cae9
1 changed files with 16 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
mission/controller/AcsController.cpp
View File

@ -169,23 +169,24 @@ void AcsController::performAttitudeControl() {
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, static_cast<uint32_t>(mekfData.mekfStatus.value));
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (mode) { switch (mode) {
case acs::SAFE: case acs::SAFE:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (submode) { switch (submode) {
case SUBMODE_NONE: case SUBMODE_NONE:
performSafe(); performSafe();
@ -200,35 +201,6 @@ void AcsController::performAttitudeControl() {
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
case acs::PTG_NADIR: case acs::PTG_NADIR:
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
0);
mekfInvalidCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
mekfInvalidCounter = 0;
}
performPointingCtrl(); performPointingCtrl();
break; break;
} }