From eb08d8849b631e9e5b08f496ddd571902da0a7b0 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 28 Sep 2022 15:27:51 +0200
Subject: [PATCH] added SUS datapool to AcsController

---
 mission/controller/AcsController.cpp          | 83 ++++++++++++++++++-
 mission/controller/AcsController.h            | 18 ++++
 .../AcsCtrlDefinitions.h                      | 17 +++-
 3 files changed, 115 insertions(+), 3 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 09e91e26..bfce1d48 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -3,7 +3,7 @@
 #include <fsfw/datapool/PoolReadGuard.h>
 
 AcsController::AcsController(object_id_t objectId)
-    : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {}
+    : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this), susData(this) {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   return returnvalue::OK;
@@ -35,6 +35,12 @@ void AcsController::performControlOperation() {
       copyMgmData();
     }
   }
+  {
+    PoolReadGuard pg(&susData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copySusData();
+    }
+  }
 }
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
@@ -94,3 +100,78 @@ void AcsController::copyMgmData() {
     }
   }
 }
+
+void AcsController::copySusData() {
+  {
+    PoolReadGuard pg(&susSets[0]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[1]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[2]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[3]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[4]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[5]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[6]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[7]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[8]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[9]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[10]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+  {
+    PoolReadGuard pg(&susSets[11]);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t));
+    }
+  }
+}
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 14b710a2..210b1c04 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -55,6 +55,9 @@ class AcsController : public ExtendedControllerBase {
   void copyMgmData();
 
   // Sun Sensors
+
+  acsctrl::SusDataRaw susData;
+
   std::array<SUS::SusDataset, 12> susSets{
       SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
       SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
@@ -70,6 +73,21 @@ class AcsController : public ExtendedControllerBase {
       SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
   };
 
+  PoolEntry<uint16_t> sus0PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus1PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus2PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus3PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus4PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus5PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus6PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus7PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus8PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus9PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus10PoolVec = PoolEntry<uint16_t>(6);
+  PoolEntry<uint16_t> sus11PoolVec = PoolEntry<uint16_t>(6);
+
+  void copySusData();
+
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
 };
diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
index 9a2b1abd..1b38e033 100644
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
@@ -60,9 +60,22 @@ class MgmDataRaw : public StaticLocalDataSet<MGM_SET_ENTRIES> {
  private:
 };
 
-class SusData : public StaticLocalDataSet<SUS_SET_ENTRIES> {
+class SusDataRaw : public StaticLocalDataSet<SUS_SET_ENTRIES> {
  public:
-  SusData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {}
+  SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {}
+
+  lp_vec_t<uint16_t, 6> sus0 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_0_N_LOC_XFYFZM_PT_XF, this);
+  lp_vec_t<uint16_t, 6> sus1 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_1_N_LOC_XBYFZM_PT_XB, this);
+  lp_vec_t<uint16_t, 6> sus2 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_2_N_LOC_XFYBZB_PT_YB, this);
+  lp_vec_t<uint16_t, 6> sus3 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_3_N_LOC_XFYBZF_PT_YF, this);
+  lp_vec_t<uint16_t, 6> sus4 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_4_N_LOC_XMYFZF_PT_ZF, this);
+  lp_vec_t<uint16_t, 6> sus5 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_5_N_LOC_XFYMZB_PT_ZB, this);
+  lp_vec_t<uint16_t, 6> sus6 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_6_R_LOC_XFYBZM_PT_XF, this);
+  lp_vec_t<uint16_t, 6> sus7 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this);
+  lp_vec_t<uint16_t, 6> sus8 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this);
+  lp_vec_t<uint16_t, 6> sus9 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this);
+  lp_vec_t<uint16_t, 6> sus10 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this);
+  lp_vec_t<uint16_t, 6> sus11 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this);
 
  private:
 };