works

2023-02-03 14:21:36 +01:00 · 2023-02-03 14:21:36 +01:00 · 4ed1e2411a
commit 4ed1e2411a
parent 76d18a67bc
6 changed files with 77 additions and 28 deletions
--- 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
--- 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};
--- 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;
 };
--- 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<ModeListEntry, 2>());
+auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_OFF_TGT =
    std::make_pair((acs::CtrlSubmode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
 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<ModeListEntry, 3>());
 auto ACS_TABLE_PTG_TARGET_GS_TGT =
    std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>());
-auto ACS_TABLE_PTG_TARGET_GS_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 =
    std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());

@ -86,8 +84,6 @@ auto ACS_SEQUENCE_PTG_TARGET_NADIR =
    std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1,
                                                     FixedArrayList<ModeListEntry, 6>());
-auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_0 = std::make_pair(
-    (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair(
    (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());

@ -95,13 +91,12 @@ auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
    std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair(
    (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
-auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_0 = std::make_pair(
-    (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair(
    (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());

 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<ModeListEntry>& 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<ModeListEntry>& sequence) {
+    eh.setObject(obj);
+    eh.setMode(mode);
+    eh.setSubmode(submode);
+    check(sequence.insert(eh), ctxc);
+  };
+  // Insert Helper Sequence
+  auto ihs = [&](ArrayList<ModeListEntry>& 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
--- a/mission/system/tree/acsModeTree.h
+++ b/mission/system/tree/acsModeTree.h
@ -1,11 +1,10 @@
-#include <fsfw/subsystem/modes/ModeDefinitions.h>
+#include <mission/system/objects/AcsSubsystem.h>

-class Subsystem;

 namespace satsystem {
 namespace acs {

-extern Subsystem ACS_SUBSYSTEM;
+extern AcsSubsystem ACS_SUBSYSTEM;
 void init();

 }  // namespace acs
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit a39e94279b0d77b9d21a996d87090d69fd4b3222
+Subproject commit c3c58b95ada024e53a019c34b91f0552bfd487a7