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 +1350 -1703
51 changed files with 1350 additions and 1703 deletions
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); navigation.resetMekf(&mekfData);
return HasActionsIF::EXECUTION_FINISHED; return HasActionsIF::EXECUTION_FINISHED;
} }
case RESTORE_MEKF_NONFINITE_RECOVERY: {
mekfLost = false;
return HasActionsIF::EXECUTION_FINISHED;
}
default: { default: {
return HasActionsIF::INVALID_ACTION_ID; return HasActionsIF::INVALID_ACTION_ID;
} }
@@ -149,6 +153,10 @@ void AcsController::performSafe() {
triggerEvent(acs::MEKF_INVALID_INFO); triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else { } else {
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
@@ -220,6 +228,9 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_INVALID_INFO); triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE) {
navigation.resetMekf(&mekfData);
}
} else { } else {
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }

4
mission/controller/AcsController.h
View File

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