fixed dummies, scheduled dummies as part of acsTask

bsp_q7s/core/InitMission.cpp

@@ -139,6 +139,25 @@ void initmission::initTasks() {
     initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER);
   }
 #endif
+#if OBSW_Q7S_EM == 1
+  acsTask->addComponent(objects::MGM_0_LIS3_HANDLER);
+  acsTask->addComponent(objects::MGM_1_RM3100_HANDLER);
+  acsTask->addComponent(objects::MGM_2_LIS3_HANDLER);
+  acsTask->addComponent(objects::MGM_3_RM3100_HANDLER);
+  acsTask->addComponent(objects::IMTQ_HANDLER);
+  acsTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
+  acsTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
+  acsTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
+  acsTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
+  acsTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
+  acsTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
+  acsTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
+  acsTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
+  acsTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
+  acsTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
+  acsTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
+  acsTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
+#endif
 
   PeriodicTaskIF* sysTask = factory->createPeriodicTask(
       "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);

dummies/ImtqDummy.cpp

@@ -41,5 +41,7 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP
   localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
   localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0,0.0,0.0}));
   localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.0,0.0,0.0}));
+  localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
   return returnvalue::OK;
 }

dummies/MgmLIS3MDLDummy.cpp

@@ -41,6 +41,6 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry<float>({0.0,0.0,0.0}));
+  localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry<float>({0.0, 0.0, 0.0}));
   return returnvalue::OK;
 }

dummies/SusDummy.cpp

@@ -54,7 +54,8 @@ void SusDummy::performControlOperation() {
 ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                 LocalDataPoolManager& poolManager) {
   localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
-  localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
+                           new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}));
 
   return returnvalue::OK;
 }

mission/controller/AcsController.cpp

@@ -1,14 +1,17 @@
 #include "AcsController.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/timemanager/Clock.h>
+//#include <fsfw/timemanager/Clock.h>
 
 AcsController::AcsController(object_id_t objectId)
     : ExtendedControllerBase(objectId, objects::NO_OBJECT),
+      /*sensorProcessing(&acsParameters),
+          navigation(&acsParameters),
+          actuatorCmd(&acsParameters),
+          detumble(&acsParameters),
+      detumbleCounter{0},*/
       mgmData(this),
-      susData(this),
+      susData(this) {}
-      sensorProcessing(&acsParameters),
-      detumbleCounter{0} {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   return returnvalue::OK;
@@ -28,13 +31,28 @@ void AcsController::performControlOperation() {
       return;
     }
     case InternalState::READY: {
+      if (mode != MODE_OFF) {
+        switch (submode) {
+          case SUBMODE_SAFE:
+            // performSafe();
+            break;
+
+          case SUBMODE_DETUMBLE:
+            // performDetumble();
+            break;
+
+          case SUBMODE_PTG_GS:
+            // performPointingCtrl();
+            break;
+        }
+      }
       break;
     }
     default:
       break;
   }
 
-  /*{
+  {
     PoolReadGuard pg(&mgmData);
     if (pg.getReadResult() == returnvalue::OK) {
       copyMgmData();
@@ -45,11 +63,15 @@ void AcsController::performControlOperation() {
     if (pg.getReadResult() == returnvalue::OK) {
       copySusData();
     }
-  }*/
+  }
+}
+
+void AcsController::performSafe(){
+
 }
 
 void AcsController::performDetumble() {
-  ACS::SensorValues sensorValues;
+  /*ACS::SensorValues sensorValues;
   ACS::OutputValues outputValues;
 
   // sensorValues.read();
@@ -82,17 +104,36 @@ void AcsController::performDetumble() {
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     submode = SUBMODE_SAFE;
     detumbleCounter = 0;
-  }
+  }*/
+}
+
+void AcsController::performPointingCtrl(){
+
 }
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
+  // MGM
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
+  // SUS
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec);
+
   poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0});
   return returnvalue::OK;
 }
@@ -124,6 +165,10 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
   return INVALID_MODE;
 }
 
+void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
+
+void AcsController::announceMode(bool recursive) {}
+
 void AcsController::copyMgmData() {
   {
     PoolReadGuard pg(&mgm0Lis3Set);

mission/controller/AcsController.h

@@ -27,16 +27,19 @@ class AcsController : public ExtendedControllerBase {
   static const Submode_t SUBMODE_PTG_NADIR = 5;
 
  protected:
+
+  void performSafe();
   void performDetumble();
+  void performPointingCtrl();
 
  private:
-  AcsParameters acsParameters;
+  /*AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
 
   Detumble detumble;
-  uint8_t detumbleCounter;
+  uint8_t detumbleCounter;*/
 
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
@@ -52,6 +55,8 @@ class AcsController : public ExtendedControllerBase {
   // Mode abstract functions
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t* msToReachTheMode) override;
+  void modeChanged(Mode_t mode, Submode_t submode);
+  void announceMode(bool recursive);
 
   // MGMs
   acsctrl::MgmDataRaw mgmData;

mission/controller/acs/AcsParameters.cpp

@@ -1,18 +1,17 @@
-#include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include "AcsParameters.h"
 
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include <stddef.h>
+
 #include <cmath>
 
 AcsParameters::AcsParameters() {}
 
 AcsParameters::~AcsParameters() {}
-
 /*
-ReturnValue_t AcsParameters::getParameter(uint8_t domainId,
+ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
-		uint16_t parameterId, ParameterWrapper* parameterWrapper,
+                                          ParameterWrapper* parameterWrapper,
-		const ParameterWrapper *newValues, uint16_t startAtIndex) {
+                                          const ParameterWrapper* newValues,
-
+                                          uint16_t startAtIndex) {
-	return 0;
+  return returnvalue::OK;
-
 }*/

mission/controller/acs/AcsParameters.h

@@ -11,7 +11,7 @@
 
 typedef unsigned char uint8_t;
 
-class AcsParameters/*:public HasParametersIF*/{
+class AcsParameters /*: public HasParametersIF*/{
 public:
 
 	AcsParameters();