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

Finite check for MEKF #465

Merged
muellerr merged 13 commits from acs-ctrl-finite-check into develop 2023-03-21 11:47:33 +01:00
Conversation 0 Commits 13 Files Changed 15 +15
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit f2210010de - Show all commits

11
mission/controller/AcsController.cpp
View File

@ -55,6 +55,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
navigation.resetMekf(&mekfData);
return HasActionsIF::EXECUTION_FINISHED;
}
case RESTORE_MEKF_NONFINITE_RECOVERY: {
mekfLost = false;
return HasActionsIF::EXECUTION_FINISHED;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
@ -149,6 +153,10 @@ void AcsController::performSafe() {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else {
mekfInvalidFlag = false;
}
@ -220,6 +228,9 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE) {
navigation.resetMekf(&mekfData);
}
} else {
mekfInvalidFlag = false;
}

4
mission/controller/AcsController.h
View File

@ -62,6 +62,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;
uint16_t mekfInvalidCounter = 0;
uint8_t resetMekfCount = 0;
bool mekfLost = false;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0};
@ -75,6 +78,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
/** Device command IDs */
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1;
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);