diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 34d29ebf..2bb536f2 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -11,10 +11,10 @@ enum CtrlSubmode { SAFE = 2, DETUMBLE = 3, IDLE = 4, - PTG_NADIR = 5, + PTG_TARGET_NADIR = 5, PTG_TARGET = 6, PTG_TARGET_GS = 7, - PTG_INERTIAL = 8, + PTG_TARGET_INERTIAL = 8, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2796bc8b..a2ff9edf 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -53,8 +53,8 @@ void AcsController::performControlOperation() { performDetumble(); break; case acs::PTG_TARGET: - case acs::PTG_NADIR: - case acs::PTG_INERTIAL: + case acs::PTG_TARGET_NADIR: + case acs::PTG_TARGET_INERTIAL: performPointingCtrl(); break; } diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index f5a1df23..6c27b160 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -20,7 +20,11 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh); void buildDetumbleSequence(Subsystem& ss, ModeListEntry& entryHelper); void buildSafeSequence(Subsystem& ss, ModeListEntry& entryHelper); void buildIdleSequence(Subsystem& ss, ModeListEntry& entryHelper); +void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh); void buildTargetPtSequence(Subsystem& ss, ModeListEntry& entryHelper); +void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& entryHelper); +void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper); + } // namespace static const auto OFF = HasModesIF::MODE_OFF; @@ -29,8 +33,10 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList()); auto ACS_TABLE_OFF_TGT = std::make_pair((acs::CtrlSubmode::OFF << 24) | 1, FixedArrayList()); -auto ACS_TABLE_OFF_TRANS = - std::make_pair((acs::CtrlSubmode::OFF << 24) | 2, FixedArrayList()); +auto ACS_TABLE_OFF_TRANS_0 = + std::make_pair((acs::CtrlSubmode::OFF << 24) | 2, FixedArrayList()); +auto ACS_TABLE_OFF_TRANS_1 = + std::make_pair((acs::CtrlSubmode::OFF << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_DETUMBLE = std::make_pair(acs::CtrlSubmode::DETUMBLE, FixedArrayList()); @@ -57,28 +63,59 @@ auto ACS_TABLE_IDLE_TRANS_0 = auto ACS_TABLE_IDLE_TRANS_1 = std::make_pair((acs::CtrlSubmode::IDLE << 24) | 3, FixedArrayList()); +auto ACS_TABLE_PTG_TRANS_0 = + std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList()); + auto ACS_SEQUENCE_PTG_TARGET = std::make_pair(acs::CtrlSubmode::PTG_TARGET, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 1, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_TRANS_0 = - std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TRANS_1 = std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 3, FixedArrayList()); -/* -auto ACS_SEQUENCE_PTG_TARGET = - std::make_pair(acs::CtrlSubmode::TARGET_PT, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TGT = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 1, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TRANS_0 = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 2, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TRANS_1 = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 3, FixedArrayList()); -*/ +auto ACS_SEQUENCE_PTG_TARGET_GS = + std::make_pair(acs::CtrlSubmode::PTG_TARGET_GS, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_GS_TGT = + std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_GS_TRANS_0 = + std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 2, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = + std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_PTG_TARGET_NADIR = + std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1, + FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_0 = std::make_pair( + (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 2, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( + (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = + std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( + (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_0 = std::make_pair( + (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 2, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( + (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + entry.setObject(obj); + entry.setMode(mode); + entry.setSubmode(submode); + check(table.insert(entry), "satsystem::acs::init: generic target"); + }; + // Build TARGET PT transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + buildOffSequence(ACS_SUBSYSTEM, entry); buildSafeSequence(ACS_SUBSYSTEM, entry); buildDetumbleSequence(ACS_SUBSYSTEM, entry); @@ -109,20 +146,24 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { }; // OFF Target table is empty - check(ss.addTable(&ACS_TABLE_OFF_TGT.second, ACS_TABLE_OFF_TGT.first, false, true), ctxc); + check(ss.addTable(TableEntry(ACS_TABLE_OFF_TGT.first, &ACS_TABLE_OFF_TGT.second)), ctxc); - // Build OFF transition - iht(objects::ACS_CONTROLLER, OFF, 0, ACS_TABLE_OFF_TRANS.second); - iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS.second); - iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS.second); - iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); - iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); - iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); - check(ss.addTable(&ACS_TABLE_OFF_TRANS.second, ACS_TABLE_OFF_TRANS.first, false, true), ctxc); + // Build OFF transition 0 + iht(objects::ACS_CONTROLLER, OFF, 0, ACS_TABLE_OFF_TRANS_0.second); + check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_0.first, &ACS_TABLE_OFF_TRANS_0.second)), ctxc); + + // Build OFF transition 1 + iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc); // Build OFF sequence ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TGT.first, 0, false); - ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TRANS.first, 0, false); + ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TRANS_0.first, 0, false); + ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TRANS_1.first, 0, false); check(ss.addSequence(&ACS_SEQUENCE_OFF.second, ACS_SEQUENCE_OFF.first, ACS_SEQUENCE_OFF.first, false, true), ctxc); @@ -148,7 +189,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build SAFE target - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::SAFE, 0, ACS_TABLE_SAFE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); @@ -164,7 +205,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build SAFE transition 1 - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::SAFE, 0, ACS_TABLE_SAFE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), ctxc); @@ -197,7 +238,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build DETUMBLE target - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::DETUMBLE, 0, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); @@ -215,7 +256,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build DETUMBLE transition 1 - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::DETUMBLE, 0, ACS_TABLE_DETUMBLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, true), ctxc); @@ -249,7 +290,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::IDLE, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -265,7 +306,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::IDLE, 0, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -297,7 +338,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); @@ -306,18 +347,10 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true), ctxc); - // Build TARGET PT transition 0 - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TRANS_0.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TRANS_0.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TRANS_0.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TRANS_0.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TRANS_0.second); - check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_0.second, ACS_TABLE_PTG_TARGET_TRANS_0.first, false, - true), - ctxc); + check(ss.addTable(&ACS_TABLE_PTG_TRANS_0.second, ACS_TABLE_PTG_TRANS_0.first, false, true), ctxc); // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, acs::CtrlSubmode::PTG_TARGET, 0, + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), @@ -325,11 +358,66 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Build IDLE sequence ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TGT.first, 0, true); - ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, 0, false); check(ss.addSequence(&ACS_SEQUENCE_PTG_TARGET.second, ACS_SEQUENCE_PTG_TARGET.first, ACS_SEQUENCE_IDLE.first, false, true), ctxc); } +void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildTargetPtNadirSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build TARGET PT table + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, + ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first, + &ACS_TABLE_PTG_TARGET_NADIR_TGT.second)), + ctxc); + + check(ss.addTable(TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)), ctxc); + + // Build TARGET PT transition 1 + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR, + ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); + check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, + &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), + ctxc); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TGT.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, 0, false); + check( + ss.addSequence(SequenceEntry(ACS_SEQUENCE_PTG_TARGET_NADIR.first, + &ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& entryHelper) {} + +void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper) {} + } // namespace