From 5d9ed222f5cba12142a55d05a7cc4f508c2b7db0 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 23 Feb 2023 11:26:43 +0100 Subject: [PATCH] added returnvalue handling --- common/config/eive/resultClassIds.h | 5 ++--- mission/controller/AcsController.cpp | 19 +++++++++++-------- mission/controller/AcsController.h | 3 +++ .../acs/MultiplicativeKalmanFilter.h | 18 +++++++++--------- mission/controller/acs/Navigation.cpp | 2 +- 5 files changed, 26 insertions(+), 21 deletions(-) diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h index e4dfb927..310d9e0b 100644 --- a/common/config/eive/resultClassIds.h +++ b/common/config/eive/resultClassIds.h @@ -35,14 +35,13 @@ enum commonClassIds : uint8_t { SA_DEPL_HANDLER, // SADPL MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF SUPV_RETURN_VALUES_IF, // SPVRTVIF - ACS_KALMAN, // ACSKAL + ACS_CTRL, // ACSCTRL + ACS_MEKF, // ACSMEKF ACS_SAFE, // ACSSAF ACS_PTG, // ACSPTG ACS_DETUMBLE, // ACSDTB - ACS_MEKF, // ACSMEK COMMON_CLASS_ID_END // [EXPORT] : [END] }; - } #endif /* COMMON_CONFIG_COMMONCLASSIDS_H_ */ diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 58c4424a..bc23ff30 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -49,7 +49,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t const uint8_t *data, size_t size) { switch (actionId) { case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: { - guidance.solarArrayDeploymentComplete(); + ReturnValue_t result = guidance.solarArrayDeploymentComplete(); + if (result == returnvalue::FAILED) { + return FILE_DELETION_FAILED; + } return HasActionsIF::EXECUTION_FINISHED; } case RESET_MEKF: { @@ -140,8 +143,8 @@ void AcsController::performSafe() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -155,7 +158,7 @@ void AcsController::performSafe() { // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; - if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) { + if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), @@ -205,8 +208,8 @@ void AcsController::performDetumble() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -252,8 +255,8 @@ void AcsController::performPointingCtrl() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index d92a189a..7bae7b36 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -70,6 +70,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; + static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; + static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); + ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index d8bcd105..edda9842 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -74,15 +74,15 @@ class MultiplicativeKalmanFilter { }; // resetting Mekf - static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; - static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2); - static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3); - static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4); - static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5); - static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED = - returnvalue::makeCode(IF_KAL_ID, 6); - static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7); - static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8); + 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_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3); + static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4); + static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5); + static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED = + returnvalue::makeCode(IF_MEKF_ID, 6); + static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); + static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); private: /*Parameters*/ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index da4a21a8..03446609 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -25,7 +25,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); - if (mekfStatus == MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED) { + if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { mekfStatus = multiplicativeKalmanFilter.init( mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),