diff --git a/bsp_q7s/core/acsModeTree.cpp b/bsp_q7s/core/acsModeTree.cpp index c097d971..58c1159a 100644 --- a/bsp_q7s/core/acsModeTree.cpp +++ b/bsp_q7s/core/acsModeTree.cpp @@ -1,6 +1,8 @@ #include +#include #include "acsModeTree.h" +#include "mission/controller/controllerdefinitions/AcsControllerDefinitions.h" #include #include #include @@ -9,50 +11,69 @@ FixedArrayList ACS_SEQUENCE_OFF = FixedArrayList ACS_TABLE_OFF_TGT = FixedArrayList(); FixedArrayList ACS_TABLE_OFF_TRANS = FixedArrayList(); -static constexpr Mode_t ACS_SEQUENCE_OFF_ID = HasModesIF::MODE_OFF; -static constexpr Mode_t ACS_TABLE_OFF_TGT_ID = 0x61000000; -static constexpr Mode_t ACS_TABLE_OFF_TRANS_ID = 0x61000001; +static constexpr Mode_t ACS_SEQUENCE_OFF_ID = acs::CtrlModes::OFF; +static constexpr Mode_t ACS_TABLE_OFF_TGT_ID = (acs::CtrlModes::OFF << 8) | 1; +static constexpr Mode_t ACS_TABLE_OFF_TRANS_ID = (acs::CtrlModes::OFF << 8) | 2; + +FixedArrayList ACS_SEQUENCE_DETUMBLE = FixedArrayList(); +FixedArrayList ACS_TABLE_DETUMBLE_TGT = FixedArrayList(); +FixedArrayList ACS_TABLE_DETUMBLE_TRANS_0 = FixedArrayList(); +FixedArrayList ACS_TABLE_DETUMBLE_TRANS_1 = FixedArrayList(); + +static constexpr Mode_t ACS_SEQUENCE_DETUMBLE_ID = acs::CtrlModes::DETUMBLE; +static constexpr Mode_t ACS_TABLE_DETUMBLE_TGT_ID = (acs::CtrlModes::DETUMBLE << 8) | 1; +static constexpr Mode_t ACS_TABLE_DETUMBLE_TRANS_0_ID = (acs::CtrlModes::DETUMBLE << 8) | 2; +static constexpr Mode_t ACS_TABLE_DETUMBLE_TRANS_1_ID = (acs::CtrlModes::DETUMBLE << 8) | 3; + +// Alias for checker function +const auto CHK = satsystem::checkInsert; +static const auto OFF = HasModesIF::MODE_OFF; +static const auto NML = DeviceHandlerIF::MODE_NORMAL; void satsystem::acsSubsystemInit() { ModeListEntry entry; Subsystem *acsSubsystem = new Subsystem(objects::ACS_SUBSYSTEM, objects::EIVE_SYSTEM, 20, 20); buildOffSequence(acsSubsystem, entry); + buildDetumbleSequence(acsSubsystem, entry); + acsSubsystem->setInitialMode(HasModesIF::MODE_OFF); } void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::buildOffSequence"; + auto ctxc = context.c_str(); // Build OFF target ss->addTable(&ACS_TABLE_OFF_TGT, ACS_TABLE_OFF_TGT_ID, false, true); // Build OFF transition eh.setObject(objects::ACS_CONTROLLER); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); eh.setObject(objects::IMTQ_HANDLER); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); eh.setObject(objects::STAR_TRACKER); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); eh.setObject(objects::ACS_BOARD_ASS); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); eh.setObject(objects::SUS_BOARD_ASS); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); eh.setObject(objects::RW_ASS); - eh.setMode(HasModesIF::MODE_OFF); + eh.setMode(OFF); eh.setSubmode(0); - ACS_TABLE_OFF_TRANS.insert(eh); + CHK(ACS_TABLE_OFF_TRANS.insert(eh), ctxc); ss->addTable(&ACS_TABLE_OFF_TRANS, ACS_TABLE_OFF_TRANS_ID, false, true); @@ -60,13 +81,91 @@ void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) { eh.setTableId(ACS_TABLE_OFF_TGT_ID); eh.setWaitSeconds(0); eh.setCheckSuccess(false); - ACS_SEQUENCE_OFF.insert(eh); + CHK(ACS_SEQUENCE_OFF.insert(eh), ctxc); eh.setTableId(ACS_TABLE_OFF_TRANS_ID); eh.setWaitSeconds(0); eh.setCheckSuccess(false); - ACS_SEQUENCE_OFF.insert(eh); + CHK(ACS_SEQUENCE_OFF.insert(eh), ctxc); ss->addSequence(&ACS_SEQUENCE_OFF, ACS_SEQUENCE_OFF_ID, ACS_SEQUENCE_OFF_ID, false, true); - ss->setInitialMode(HasModesIF::MODE_OFF); +} + +void satsystem::buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { + + std::string context = "satsystem::buildDetumbleSequence"; + auto ctxc = context.c_str(); + // Build DETUMBLE target + eh.setObject(objects::ACS_CONTROLLER); + eh.setMode(acs::CtrlModes::DETUMBLE); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TGT.insert(eh), ctxc); + + eh.setObject(objects::IMTQ_HANDLER); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TGT.insert(eh), ctxc); + + eh.setObject(objects::SUS_BOARD_ASS); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TGT.insert(eh), ctxc); + + eh.setObject(objects::ACS_BOARD_ASS); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TGT.insert(eh), ctxc); + + ss->addTable(&ACS_TABLE_DETUMBLE_TGT, ACS_TABLE_DETUMBLE_TGT_ID, false, true); + + // Build DETUMBLE transition 0 + + eh.setObject(objects::IMTQ_HANDLER); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_0.insert(eh), ctxc); + + eh.setObject(objects::STAR_TRACKER); + eh.setMode(OFF); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_0.insert(eh), ctxc); + + eh.setObject(objects::ACS_BOARD_ASS); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_0.insert(eh), ctxc); + + eh.setObject(objects::SUS_BOARD_ASS); + eh.setMode(NML); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_0.insert(eh), ctxc); + + eh.setObject(objects::RW_ASS); + eh.setMode(OFF); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_0.insert(eh), ctxc); + + ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_0, ACS_TABLE_DETUMBLE_TRANS_0_ID, false, true); + + // Build DETUMBLE transition 1 + + eh.setObject(objects::ACS_CONTROLLER); + eh.setMode(acs::CtrlModes::DETUMBLE); + eh.setSubmode(0); + CHK(ACS_TABLE_DETUMBLE_TRANS_1.insert(eh), ctxc); + ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_1, ACS_TABLE_DETUMBLE_TRANS_1_ID, false, true); + + ss->addSequence(&ACS_SEQUENCE_DETUMBLE, ACS_SEQUENCE_DETUMBLE_ID, 0 /*Ü TODO*/, false, true); +} + +void satsystem::buildSafeSequence(Subsystem* ss, ModeListEntry& eh) {} + +void satsystem::buildIdleSequence(Subsystem* ss, ModeListEntry& eh) {} + +void satsystem::buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) {} + +void satsystem::checkInsert(ReturnValue_t result, const char* ctx) { + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "Insertion failed at " << ctx << std::endl; + } } diff --git a/bsp_q7s/core/acsModeTree.h b/bsp_q7s/core/acsModeTree.h index 34811349..8f0c593d 100644 --- a/bsp_q7s/core/acsModeTree.h +++ b/bsp_q7s/core/acsModeTree.h @@ -6,5 +6,10 @@ namespace satsystem { void acsSubsystemInit(); void buildOffSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildDetumbleSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildSafeSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildIdleSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildTargetPtSequence(Subsystem* ss, ModeListEntry& entryHelper); +void checkInsert(ReturnValue_t result, const char* ctx); } diff --git a/generators/fsfwgen b/generators/fsfwgen index 19e8ee90..9d7963f3 160000 --- a/generators/fsfwgen +++ b/generators/fsfwgen @@ -1 +1 @@ -Subproject commit 19e8ee90be12f72ceb8c2aefdfd665b5babad417 +Subproject commit 9d7963f3292c669814810adf50d6820b2e5c29b7 diff --git a/mission/controller/controllerdefinitions/AcsControllerDefinitions.h b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h new file mode 100644 index 00000000..43cce539 --- /dev/null +++ b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h @@ -0,0 +1,21 @@ +#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ +#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ + +#include + +namespace acs { + +enum CtrlModes { + OFF = HasModesIF::MODE_OFF, + IDLE = 1, + DETUMBLE = 2, + SAFE = 3, + IDLE = 4, + TARGET_PT = 5 +}; + +} + + + +#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ */