From b261a4ecfafafe58bdbc08c0405315b0f90a68d8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Mar 2023 18:09:50 +0100 Subject: [PATCH] works --- bsp_q7s/core/ObjectFactory.cpp | 3 ++ .../startracker/StarTrackerHandler.cpp | 2 +- mission/acsDefs.h | 2 ++ mission/controller/AcsController.cpp | 7 +++-- mission/controller/AcsController.h | 1 + mission/devices/RwHandler.cpp | 5 ++-- mission/system/fdir/CMakeLists.txt | 5 ++-- mission/system/fdir/StrFdir.cpp | 14 ++++++++++ mission/system/fdir/StrFdir.h | 12 ++++++++ mission/system/objects/AcsSubsystem.cpp | 4 +-- mission/system/objects/StrAssembly.cpp | 2 +- mission/system/tree/acsModeTree.cpp | 1 + mission/system/tree/system.cpp | 28 ++++++++----------- 13 files changed, 56 insertions(+), 30 deletions(-) create mode 100644 mission/system/fdir/StrFdir.cpp create mode 100644 mission/system/fdir/StrFdir.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 583c74b8..d078916f 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -925,11 +926,13 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { if (paramJsonFile == nullptr) { sif::error << "No valid Star Tracker parameter JSON file" << std::endl; } + auto strFdir = new StrFdir(objects::STAR_TRACKER); auto starTracker = new StarTrackerHandler(objects::STAR_TRACKER, objects::UART_COM_IF, starTrackerCookie, paramJsonFile, strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V); starTracker->setPowerSwitcher(pwrSwitcher); starTracker->connectModeTreeParent(*strAssy); + starTracker->setCustomFdir(strFdir); } void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index ef88ea67..0edf6de1 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -1677,7 +1677,7 @@ void StarTrackerHandler::prepareHistogramRequest() { ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet) { - Stopwatch watch; + // Stopwatch watch; ReturnValue_t result = returnvalue::OK; if (commandDataLen > MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 61a3c644..9b7916af 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -18,6 +18,8 @@ enum AcsMode : Mode_t { PTG_INERTIAL = 16, }; +// static constexpr uint8_t ACS_SYSTEM_DETUMBLE_SUBMODE = 1; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //!< The limits for the rotation in safe mode were violated. static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b40302a9..25ad8706 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -264,8 +264,9 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } - if (mekfInvalidCounter > 4) { - triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION); + if (mekfInvalidCounter == 5) { + // Trigger this so STR FDIR can set the device faulty. + EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); } mekfInvalidCounter++; // commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, @@ -281,7 +282,7 @@ void AcsController::performPointingCtrl() { double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - if (multipleRwUnavailableCounter > 4) { + if (multipleRwUnavailableCounter == 5) { triggerEvent(acs::MULTIPLE_RW_INVALID); } multipleRwUnavailableCounter++; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f59a4605..91ca32d6 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 6d6032b3..7f82b202 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -366,9 +366,8 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); if (statusSet.state == rws::STATE_ERROR) { - // This requires the commanding of the init reaction wheel controller command to recover - // from error state which must be handled by the FDIR instance. - triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0); + // Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue. + triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0); sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl; } diff --git a/mission/system/fdir/CMakeLists.txt b/mission/system/fdir/CMakeLists.txt index 37b2c290..34a7e125 100644 --- a/mission/system/fdir/CMakeLists.txt +++ b/mission/system/fdir/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources( - ${LIB_EIVE_MISSION} PRIVATE AcsBoardFdir.cpp RtdFdir.cpp SusFdir.cpp - SyrlinksFdir.cpp GomspacePowerFdir.cpp) + ${LIB_EIVE_MISSION} + PRIVATE AcsBoardFdir.cpp RtdFdir.cpp StrFdir.cpp SusFdir.cpp SyrlinksFdir.cpp + GomspacePowerFdir.cpp) diff --git a/mission/system/fdir/StrFdir.cpp b/mission/system/fdir/StrFdir.cpp new file mode 100644 index 00000000..7d0947a9 --- /dev/null +++ b/mission/system/fdir/StrFdir.cpp @@ -0,0 +1,14 @@ +#include "StrFdir.h" + +#include "mission/acsDefs.h" + +StrFdir::StrFdir(object_id_t strObject) + : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} + +ReturnValue_t StrFdir::eventReceived(EventMessage* event) { + if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + setFaulty(event->getEvent()); + return returnvalue::OK; + } + return DeviceHandlerFailureIsolation::eventReceived(event); +} diff --git a/mission/system/fdir/StrFdir.h b/mission/system/fdir/StrFdir.h new file mode 100644 index 00000000..20476e1a --- /dev/null +++ b/mission/system/fdir/StrFdir.h @@ -0,0 +1,12 @@ +#ifndef MISSION_SYSTEM_FDIR_STRFDIR_H_ +#define MISSION_SYSTEM_FDIR_STRFDIR_H_ + +#include + +class StrFdir : public DeviceHandlerFailureIsolation { + public: + StrFdir(object_id_t strObject); + ReturnValue_t eventReceived(EventMessage* event) override; +}; + +#endif /* MISSION_SYSTEM_FDIR_STRFDIR_H_ */ diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index c812394c..e4969ac1 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -75,9 +75,7 @@ void AcsSubsystem::handleEventMessages() { sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; } } - if (event.getEvent() == acs::SAFE_RATE_RECOVERY || - event.getEvent() == acs::MULTIPLE_RW_INVALID || - event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { CommandMessage msg; ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); status = commandQueue->sendMessage(commandQueue->getId(), &msg); diff --git a/mission/system/objects/StrAssembly.cpp b/mission/system/objects/StrAssembly.cpp index 6162ecf0..10ca5759 100644 --- a/mission/system/objects/StrAssembly.cpp +++ b/mission/system/objects/StrAssembly.cpp @@ -2,7 +2,7 @@ #include -StrAssembly::StrAssembly(object_id_t objectId): AssemblyBase(objectId) { +StrAssembly::StrAssembly(object_id_t objectId) : AssemblyBase(objectId) { ModeListEntry entry; entry.setObject(objects::STAR_TRACKER); entry.setMode(MODE_OFF); diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 086bcda4..ce419d0b 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -319,6 +319,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp index 195b6905..d1b127d4 100644 --- a/mission/system/tree/system.cpp +++ b/mission/system/tree/system.cpp @@ -62,10 +62,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { std::string context = "satsystem::buildSafeSequence"; auto ctxc = context.c_str(); // Insert Helper Table - auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table, + bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(table.insert(eh), ctxc); }; // Insert Helper Sequence @@ -79,7 +83,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { // Do no track ACS for now because it might jump to detumble mode and back to safe as part of // normal operations. - // iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second); + // iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true); iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc); @@ -88,18 +92,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second); iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second); iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second); - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)), ctxc); - // Build SAFE transition 1 - // check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &EIVE_TABLE_SAFE_TRANS_1.second)), - // ctxc); - // Build Safe sequence ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false); ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false); - // ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_1.first, 0, false); check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second, EIVE_SEQUENCE_SAFE.first)), ctxc); @@ -127,21 +126,16 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second); check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc); - // Build SAFE transition 0 + // Build IDLE transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_1.second); - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_1.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)), ctxc); - // Build SAFE transition 1 - // check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)), - // ctxc); - - // Build Safe sequence + // Build IDLE sequence ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false); ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false); - // ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_1.first, 0, false); check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second, EIVE_SEQUENCE_SAFE.first)), ctxc);