From 9ba65b04fd703d55518f8506e6aa6ce143205ebe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 25 Apr 2022 10:30:47 +0200 Subject: [PATCH] add IDLE charge mode --- bsp_q7s/core/acsModeTree.cpp | 224 ++++++++++++------ bsp_q7s/core/acsModeTree.h | 1 + .../AcsControllerDefinitions.h | 6 +- 3 files changed, 154 insertions(+), 77 deletions(-) diff --git a/bsp_q7s/core/acsModeTree.cpp b/bsp_q7s/core/acsModeTree.cpp index f99b79fc..b944caa9 100644 --- a/bsp_q7s/core/acsModeTree.cpp +++ b/bsp_q7s/core/acsModeTree.cpp @@ -13,60 +13,66 @@ const auto CHK = satsystem::checkInsert; static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlModes::OFF, FixedArrayList()); +auto ACS_SEQUENCE_OFF = + std::make_pair(acs::CtrlModes::OFF << 24, FixedArrayList()); auto ACS_TABLE_OFF_TGT = - std::make_pair((acs::CtrlModes::OFF << 8) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::OFF << 24) | 1, FixedArrayList()); auto ACS_TABLE_OFF_TRANS = - std::make_pair((acs::CtrlModes::OFF << 8) | 2, FixedArrayList()); + std::make_pair((acs::CtrlModes::OFF << 24) | 2, FixedArrayList()); auto ACS_SEQUENCE_DETUMBLE = - std::make_pair(acs::CtrlModes::DETUMBLE, FixedArrayList()); + std::make_pair(acs::CtrlModes::DETUMBLE << 24, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TGT = - std::make_pair((acs::CtrlModes::DETUMBLE << 8) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_0 = - std::make_pair((acs::CtrlModes::DETUMBLE << 8) | 2, FixedArrayList()); + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_1 = - std::make_pair((acs::CtrlModes::DETUMBLE << 8) | 3, FixedArrayList()); + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_SAFE = - std::make_pair(acs::CtrlModes::SAFE, FixedArrayList()); + std::make_pair(acs::CtrlModes::SAFE << 24, FixedArrayList()); auto ACS_TABLE_SAFE_TGT = - std::make_pair((acs::CtrlModes::SAFE << 8) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::SAFE << 24) | 1, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_0 = - std::make_pair((acs::CtrlModes::SAFE << 8) | 2, FixedArrayList()); + std::make_pair((acs::CtrlModes::SAFE << 24) | 2, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_1 = - std::make_pair((acs::CtrlModes::SAFE << 8) | 3, FixedArrayList()); + std::make_pair((acs::CtrlModes::SAFE << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_IDLE = - std::make_pair(acs::CtrlModes::IDLE, FixedArrayList()); + std::make_pair(acs::CtrlModes::IDLE << 24, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::CtrlModes::IDLE << 8) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::CtrlModes::IDLE << 8) | 2, FixedArrayList()); + std::make_pair((acs::CtrlModes::IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::CtrlModes::IDLE << 8) | 3, FixedArrayList()); + std::make_pair((acs::CtrlModes::IDLE << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_IDLE_CHRG = std::make_pair(acs::CtrlModes::IDLE << 24 | (acs::IDLE_CHARGE << 8), + FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TGT = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 1, FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TRANS_0 = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 2, FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TRANS_1 = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 3, FixedArrayList()); auto ACS_SEQUENCE_TARGET_PT = std::make_pair(acs::CtrlModes::TARGET_PT, FixedArrayList()); auto ACS_TABLE_TARGET_PT_TGT = - std::make_pair((acs::CtrlModes::TARGET_PT << 8) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 1, FixedArrayList()); auto ACS_TABLE_TARGET_PT_TRANS_0 = - std::make_pair((acs::CtrlModes::TARGET_PT << 8) | 2, FixedArrayList()); + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 2, FixedArrayList()); auto ACS_TABLE_TARGET_PT_TRANS_1 = - std::make_pair((acs::CtrlModes::TARGET_PT << 8) | 3, FixedArrayList()); - -enum InsertType { - TABLE, - SEQ -}; + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 3, FixedArrayList()); void satsystem::acsSubsystemInit() { ModeListEntry entry; - Subsystem* acsSubsystem = new Subsystem(objects::ACS_SUBSYSTEM, objects::EIVE_SYSTEM, 20, 20); + Subsystem* acsSubsystem = new Subsystem(objects::ACS_SUBSYSTEM, objects::EIVE_SYSTEM, 12, 24); buildOffSequence(acsSubsystem, entry); buildSafeSequence(acsSubsystem, entry); buildDetumbleSequence(acsSubsystem, entry); buildIdleSequence(acsSubsystem, entry); + buildIdleChargeSequence(acsSubsystem, entry); buildTargetPtSequence(acsSubsystem, entry); acsSubsystem->setInitialMode(HasModesIF::MODE_OFF); } @@ -75,16 +81,15 @@ void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) { std::string context = "satsystem::buildOffSequence"; 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) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); CHK(table.insert(eh), ctxc); }; // Insert Helper Sequence - auto ihs = [&]( ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { eh.setTableId(tableId); eh.setWaitSeconds(waitSeconds); eh.setCheckSuccess(checkSuccess); @@ -92,7 +97,7 @@ void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) { }; // OFF Target table is empty - ss->addTable(&ACS_TABLE_OFF_TGT.second, ACS_TABLE_OFF_TGT.first, false, true); + CHK(ss->addTable(&ACS_TABLE_OFF_TGT.second, ACS_TABLE_OFF_TGT.first, false, true), ctxc); // Build OFF transition iht(objects::ACS_CONTROLLER, OFF, 0, ACS_TABLE_OFF_TRANS.second); @@ -101,13 +106,14 @@ void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) { 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); - ss->addTable(&ACS_TABLE_OFF_TRANS.second, ACS_TABLE_OFF_TRANS.first, false, true); + CHK(ss->addTable(&ACS_TABLE_OFF_TRANS.second, ACS_TABLE_OFF_TRANS.first, false, true), 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); - ss->addSequence(&ACS_SEQUENCE_OFF.second, ACS_SEQUENCE_OFF.first, ACS_SEQUENCE_OFF.first, false, - true); + CHK(ss->addSequence(&ACS_SEQUENCE_OFF.second, ACS_SEQUENCE_OFF.first, ACS_SEQUENCE_OFF.first, + false, true), + ctxc); } void satsystem::buildSafeSequence(Subsystem* ss, ModeListEntry& eh) { @@ -115,15 +121,15 @@ void satsystem::buildSafeSequence(Subsystem* ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); CHK(sequence.insert(eh), ctxc); }; // Insert Helper Sequence - auto ihs = [&]( ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { eh.setTableId(tableId); eh.setWaitSeconds(waitSeconds); eh.setCheckSuccess(checkSuccess); @@ -134,7 +140,7 @@ void satsystem::buildSafeSequence(Subsystem* ss, ModeListEntry& eh) { 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); - ss->addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_OFF_TGT.first, false, true); + CHK(ss->addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_OFF_TGT.first, false, true), ctxc); // Build SAFE transition 0 iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); @@ -142,18 +148,21 @@ void satsystem::buildSafeSequence(Subsystem* ss, ModeListEntry& eh) { iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::RW_ASS, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); - ss->addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true); + CHK(ss->addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true), + ctxc); // Build SAFE transition 1 iht(objects::ACS_CONTROLLER, acs::CtrlModes::SAFE, 0, ACS_TABLE_SAFE_TRANS_1.second); - ss->addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true); + CHK(ss->addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), + ctxc); // Build SAFE sequence - ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TGT.first, 0, false); + ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TGT.first, 0, true); ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_0.first, 0, false); ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_1.first, 0, false); - ss->addSequence(&ACS_SEQUENCE_SAFE.second, ACS_SEQUENCE_SAFE.first, ACS_SEQUENCE_OFF.first, false, - true); + CHK(ss->addSequence(&ACS_SEQUENCE_SAFE.second, ACS_SEQUENCE_SAFE.first, ACS_SEQUENCE_OFF.first, + false, true), + ctxc); } void satsystem::buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { @@ -161,15 +170,15 @@ void satsystem::buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); CHK(sequence.insert(eh), ctxc); }; // Insert Helper Sequence - auto ihs = [&]( ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { eh.setTableId(tableId); eh.setWaitSeconds(waitSeconds); eh.setCheckSuccess(checkSuccess); @@ -180,7 +189,8 @@ void satsystem::buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { 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); - ss->addTable(&ACS_TABLE_DETUMBLE_TGT.second, ACS_TABLE_DETUMBLE_TGT.first, false, true); + CHK(ss->addTable(&ACS_TABLE_DETUMBLE_TGT.second, ACS_TABLE_DETUMBLE_TGT.first, false, true), + ctxc); // Build DETUMBLE transition 0 iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); @@ -188,35 +198,39 @@ void satsystem::buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::RW_ASS, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); - ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false, true); + CHK(ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false, + true), + ctxc); // Build DETUMBLE transition 1 iht(objects::ACS_CONTROLLER, acs::CtrlModes::DETUMBLE, 0, ACS_TABLE_DETUMBLE_TRANS_1.second); - ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, true); + CHK(ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, + true), + ctxc); // Build DETUMBLE sequence - ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TGT.first, 0, false); + ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TGT.first, 0, true); ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_0.first, 0, false); ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_1.first, 0, false); - ss->addSequence(&ACS_SEQUENCE_DETUMBLE.second, ACS_SEQUENCE_DETUMBLE.first, - ACS_SEQUENCE_DETUMBLE.first, false, true); + CHK(ss->addSequence(&ACS_SEQUENCE_DETUMBLE.second, ACS_SEQUENCE_DETUMBLE.first, + ACS_SEQUENCE_SAFE.first, false, true), + ctxc); } - void satsystem::buildIdleSequence(Subsystem* ss, ModeListEntry& eh) { std::string context = "satsystem::buildIdleSequence"; auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); CHK(sequence.insert(eh), ctxc); }; // Insert Helper Sequence - auto ihs = [&]( ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { eh.setTableId(tableId); eh.setWaitSeconds(waitSeconds); eh.setCheckSuccess(checkSuccess); @@ -231,23 +245,77 @@ void satsystem::buildIdleSequence(Subsystem* ss, ModeListEntry& eh) { ss->addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); // Build IDLE transition 0 - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); - iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_IDLE_TRANS_0.second); 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::CtrlModes::IDLE, 0, ACS_TABLE_DETUMBLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, 0, 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 - ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TGT.first, 0, false); - ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_0.first, 0, false); + ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TGT.first, 0, true); + ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_1.first, 0, false); - ss->addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, - ACS_SEQUENCE_IDLE.first, false, true); + ss->addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, ACS_SEQUENCE_SAFE.first, + false, true); +} + +void satsystem::buildIdleChargeSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::buildIdleChargeSequence"; + 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); + CHK(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); + CHK(sequence.insert(eh), ctxc); + }; + // Build IDLE target + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, acs::IDLE_CHARGE, + ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + CHK(ss->addTable(&ACS_TABLE_IDLE_CHRG_TGT.second, ACS_TABLE_IDLE_CHRG_TGT.first, false, true), + ctxc); + + // Build IDLE transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + CHK(ss->addTable(&ACS_TABLE_IDLE_CHRG_TRANS_0.second, ACS_TABLE_IDLE_CHRG_TRANS_0.first, false, + true), + ctxc); + + // Build IDLE transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, acs::IDLE_CHARGE, + ACS_TABLE_IDLE_CHRG_TRANS_1.second); + CHK(ss->addTable(&ACS_TABLE_IDLE_CHRG_TRANS_1.second, ACS_TABLE_IDLE_CHRG_TRANS_1.first, false, + true), + ctxc); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TGT.first, 0, true); + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TRANS_1.first, 0, false); + CHK(ss->addSequence(&ACS_SEQUENCE_IDLE_CHRG.second, ACS_SEQUENCE_IDLE_CHRG.first, + ACS_SEQUENCE_SAFE.first, false, true), + ctxc); } void satsystem::buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) { @@ -255,15 +323,15 @@ void satsystem::buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); CHK(sequence.insert(eh), ctxc); }; // Insert Helper Sequence - auto ihs = [&]( ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { eh.setTableId(tableId); eh.setWaitSeconds(waitSeconds); eh.setCheckSuccess(checkSuccess); @@ -277,7 +345,8 @@ void satsystem::buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) { iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); - ss->addTable(&ACS_TABLE_TARGET_PT_TGT.second, ACS_TABLE_TARGET_PT_TGT.first, false, true); + CHK(ss->addTable(&ACS_TABLE_TARGET_PT_TGT.second, ACS_TABLE_TARGET_PT_TGT.first, false, true), + ctxc); // Build TARGET PT transition 0 iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); @@ -285,22 +354,27 @@ void satsystem::buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) { iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); - ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_0.second, ACS_TABLE_TARGET_PT_TRANS_0.first, false, true); + CHK(ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_0.second, ACS_TABLE_TARGET_PT_TRANS_0.first, false, + true), + ctxc); // Build TARGET PT transition 1 iht(objects::ACS_CONTROLLER, acs::CtrlModes::TARGET_PT, 0, ACS_TABLE_TARGET_PT_TRANS_1.second); - ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_1.second, ACS_TABLE_TARGET_PT_TRANS_1.first, false, true); + CHK(ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_1.second, ACS_TABLE_TARGET_PT_TRANS_1.first, false, + true), + ctxc); // Build IDLE sequence - ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TGT.first, 0, false); - ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TRANS_0.first, 0, false); + ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TGT.first, 0, true); + ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TRANS_1.first, 0, false); - ss->addSequence(&ACS_SEQUENCE_TARGET_PT.second, ACS_SEQUENCE_TARGET_PT.first, - ACS_SEQUENCE_TARGET_PT.first, false, true); + CHK(ss->addSequence(&ACS_SEQUENCE_TARGET_PT.second, ACS_SEQUENCE_TARGET_PT.first, + ACS_SEQUENCE_IDLE.first, false, true), + ctxc); } void satsystem::checkInsert(ReturnValue_t result, const char* ctx) { if (result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "Insertion failed at " << ctx << std::endl; + sif::warning << "satsystem::checkInsert: Insertion failed at " << ctx << std::endl; } } diff --git a/bsp_q7s/core/acsModeTree.h b/bsp_q7s/core/acsModeTree.h index f37d0f75..b6c96e39 100644 --- a/bsp_q7s/core/acsModeTree.h +++ b/bsp_q7s/core/acsModeTree.h @@ -9,6 +9,7 @@ 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 buildIdleChargeSequence(Subsystem* ss, ModeListEntry& entryHelper); void buildTargetPtSequence(Subsystem* ss, ModeListEntry& entryHelper); void checkInsert(ReturnValue_t result, const char* ctx); diff --git a/mission/controller/controllerdefinitions/AcsControllerDefinitions.h b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h index d0733046..8122178d 100644 --- a/mission/controller/controllerdefinitions/AcsControllerDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h @@ -5,8 +5,10 @@ namespace acs { -enum CtrlModes { OFF = HasModesIF::MODE_OFF, DETUMBLE = 2, SAFE = 3, IDLE = 4, TARGET_PT = 5 }; +enum CtrlModes { OFF = HasModesIF::MODE_OFF, SAFE = 1, DETUMBLE = 2, IDLE = 3, TARGET_PT = 4 }; -} +static constexpr Submode_t IDLE_CHARGE = 1; + +} // namespace acs #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ */