From 4ed1e2411a4e79559780d294d6b658d1005ee31b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Feb 2023 14:21:36 +0100 Subject: [PATCH] works --- CHANGELOG.md | 7 +++ mission/controller/AcsController.cpp | 7 +-- mission/controller/acs/AcsParameters.h | 4 +- mission/system/tree/acsModeTree.cpp | 80 ++++++++++++++++++++------ mission/system/tree/acsModeTree.h | 5 +- tmtc | 2 +- 6 files changed, 77 insertions(+), 28 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2f36f965..c7d8bbd6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,13 @@ change warranting a new major release: # [unreleased] +- `AcsSubsystem`: OFF, SAFE and DETUMBLE mode were tested. Auto-transitions SAFE <-> DETUMBLE tested + as well. Other modes still need to be tested. + +## Fixed + +- `AcsController`: Parameter fix in `DetumbleParameter`. + # [v1.23.1] 2023-02-02 TMTC rev: 15adb9bf2ec68304a4f87b8dd418c1a8353283a3 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index a2ff9edf..8ed396c5 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -152,9 +152,8 @@ void AcsController::performSafe() { detumbleCounter = 0; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - // TODO: Trigger mode transition in ACS subsystem? - submode = acs::CtrlSubmode::DETUMBLE; detumbleCounter = 0; + // Triggers detubmle mode transition in subsystem triggerEvent(acs::SAFE_RATE_VIOLATION); } @@ -210,9 +209,9 @@ void AcsController::performDetumble() { detumbleCounter = 0; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - // TODO: Trigger mode transition in subsystem instead - submode = acs::CtrlSubmode::DETUMBLE; detumbleCounter = 0; + // Triggers safe mode transition in subsystem + triggerEvent(acs::SAFE_RATE_RECOVERY); } int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 816e4c71..5d40f752 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -899,8 +899,8 @@ class AcsParameters /*: public HasParametersIF*/ { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 0.4 * M_PI / 180; + double omegaDetumbleStart = 2; + double omegaDetumbleEnd = 0.4; double gainD = pow(10.0, -3.3); } detumbleParameter; }; diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 94c4a5c9..849959f9 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -10,7 +10,7 @@ #include "mission/acsDefs.h" #include "util.h" -Subsystem satsystem::acs::ACS_SUBSYSTEM(objects::ACS_SUBSYSTEM, 12, 24); +AcsSubsystem satsystem::acs::ACS_SUBSYSTEM(objects::ACS_SUBSYSTEM, 12, 24); namespace { // Alias for checker function @@ -30,7 +30,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper); static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList()); +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_0 = @@ -77,8 +77,6 @@ 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()); @@ -86,8 +84,6 @@ 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()); @@ -95,13 +91,12 @@ 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; + const char* ctxc = "satsystem::acs::init: generic target"; // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { entry.setObject(obj); @@ -115,6 +110,9 @@ void satsystem::acs::init() { 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); + check(ACS_SUBSYSTEM.addTable( + TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)), + ctxc); buildOffSequence(ACS_SUBSYSTEM, entry); buildSafeSequence(ACS_SUBSYSTEM, entry); @@ -350,8 +348,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true), ctxc); - check(ss.addTable(&ACS_TABLE_PTG_TRANS_0.second, ACS_TABLE_PTG_TRANS_0.first, false, true), ctxc); - + // Transition 0 already built // Build TARGET PT transition 1 iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); @@ -400,8 +397,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { &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); - + // Transition 0 already built // Build TARGET PT transition 1 iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); @@ -411,7 +407,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // 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_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, 0, true); check( ss.addSequence(SequenceEntry(ACS_SEQUENCE_PTG_TARGET_NADIR.first, @@ -441,7 +437,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { // Build TARGET PT table iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, - ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); @@ -451,8 +447,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)), ctxc); - check(ss.addTable(TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)), ctxc); - + // Transition 0 already built // Build TARGET PT transition 1 iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, ACS_TABLE_PTG_TARGET_GS_TRANS_1.second); @@ -462,13 +457,62 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { // Build IDLE sequence ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TARGET_GS_TGT.first, 0, true); - ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TARGET_GS_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, 0, true); check(ss.addSequence(SequenceEntry(ACS_SEQUENCE_PTG_TARGET_GS.first, &ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_SEQUENCE_IDLE.first)), ctxc); } -void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper) {} +void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildTargetPtInertialSequence"; + 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_INERTIAL, + ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, + &ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)), + ctxc); + + // Transition 0 already built + // Build TARGET PT transition 1 + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, + ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); + check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, + &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), + ctxc); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, 0, + true); + check(ss.addSequence(SequenceEntry(ACS_SEQUENCE_PTG_TARGET_INERTIAL.first, + &ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, + ACS_SEQUENCE_IDLE.first)), + ctxc); +} } // namespace diff --git a/mission/system/tree/acsModeTree.h b/mission/system/tree/acsModeTree.h index 210f8dcd..c814ca3a 100644 --- a/mission/system/tree/acsModeTree.h +++ b/mission/system/tree/acsModeTree.h @@ -1,11 +1,10 @@ -#include +#include -class Subsystem; namespace satsystem { namespace acs { -extern Subsystem ACS_SUBSYSTEM; +extern AcsSubsystem ACS_SUBSYSTEM; void init(); } // namespace acs diff --git a/tmtc b/tmtc index a39e9427..c3c58b95 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a39e94279b0d77b9d21a996d87090d69fd4b3222 +Subproject commit c3c58b95ada024e53a019c34b91f0552bfd487a7