continued ACS controller impl

fsfw

@@ -1 +1 @@
-Subproject commit fdcfd89ed271118ddb2daf0de3db5d69bdbc95d4
+Subproject commit 7f89022f5ba90d48abd8ba5508e3d6840ce47d74

mission/controller/AcsController.cpp

@@ -1,13 +1,34 @@
 #include "AcsController.h"
 
 AcsController::AcsController(object_id_t objectId)
-    : ExtendedControllerBase(objectId, objects::NO_OBJECT) {}
+    : ExtendedControllerBase(objectId, objects::NO_OBJECT),
+      mgmData(this) {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   return HasReturnvaluesIF::RETURN_OK;
 }
 
-void AcsController::performControlOperation() {}
+void AcsController::performControlOperation() {
+  switch (internalState) {
+    case InternalState::STARTUP: {
+      initialCountdown.resetTimer();
+      internalState = InternalState::INITIAL_DELAY;
+      return;
+    }
+    case InternalState::INITIAL_DELAY: {
+      if (initialCountdown.hasTimedOut()) {
+        internalState = InternalState::READY;
+      }
+      return;
+    }
+    case InternalState::READY: {
+      break;
+    }
+    default:
+      break;
+  }
+
+}
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
@@ -20,3 +41,6 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
                                               uint32_t *msToReachTheMode) {
   return HasReturnvaluesIF::RETURN_OK;
 }
+
+void AcsController::copyMgmData() {
+}

mission/controller/AcsController.h

@@ -1,13 +1,24 @@
 #ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
 #define MISSION_CONTROLLER_ACSCONTROLLER_H_
 
+#include <commonObjects.h>
+#include "controllerdefinitions/AcsCtrlDefinitions.h"
 #include <fsfw/controller/ExtendedControllerBase.h>
+#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
+#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
+#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
 
 class AcsController : public ExtendedControllerBase {
  public:
+  static constexpr dur_millis_t INIT_DELAY = 500;
+
   AcsController(object_id_t objectId);
 
  private:
+  enum class InternalState { STARTUP, INITIAL_DELAY, READY };
+
+  InternalState internalState = InternalState::STARTUP;
+
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
 
@@ -20,7 +31,18 @@ class AcsController : public ExtendedControllerBase {
                                  uint32_t* msToReachTheMode) override;
 
   // MGMs
+  acsctrl::MgmData mgmData;
 
+  MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
+  RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
+  MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
+  RM3100::Rm3100PrimaryDataset mgm2Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
+  IMTQ::CalibratedMtmMeasurementSet imtqMgmSet = IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER);
+
+  void copyMgmData();
+
+  // Initial delay to make sure all pool variables have been initialized their owners
+  Countdown initialCountdown = Countdown(INIT_DELAY);
   // Sun Sensors
 };
 

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -0,0 +1,48 @@
+#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
+#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
+
+#include <fsfw/datapoollocal/localPoolDefinitions.h>
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <cstdint>
+
+namespace acsctrl {
+
+enum SetIds : uint32_t {
+  MGM_SENSOR_DATA
+};
+
+enum PoolIds : lp_id_t {
+  MGM_0_LIS3_UT,
+  MGM_1_RM3100_UT,
+  MGM_2_LIS3_UT,
+  MGM_3_RM3100_UT,
+  MGM_IMTQ_CAL_NT,
+  MGM_IMTQ_CAL_ACT_STATUS
+};
+
+
+static constexpr uint8_t MGM_SET_ENTRIES = 5;
+
+/**
+ * @brief   This dataset can be used to store the collected temperatures of all temperature sensors
+ */
+class MgmData : public StaticLocalDataSet<MGM_SET_ENTRIES> {
+public:
+  MgmData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {}
+
+  // The ACS board measurement are in floating point uT
+  lp_vec_t<float, 3> mgm0Lis3 = lp_vec_t<float, 3>(sid.objectId, MGM_0_LIS3_UT, this);
+  lp_vec_t<float, 3> mgm1Rm3100 = lp_vec_t<float, 3>(sid.objectId, MGM_1_RM3100_UT, this);
+  lp_vec_t<float, 3> mgm2Lis3 = lp_vec_t<float, 3>(sid.objectId, MGM_2_LIS3_UT, this);
+  lp_vec_t<float, 3> mgm3Rm3100 = lp_vec_t<float, 3>(sid.objectId, MGM_3_RM3100_UT, this);
+  // The IMTQ measurements are in integer nT
+  lp_vec_t<int32_t, 3> imtqCal = lp_vec_t<int32_t, 3>(sid.objectId, MGM_IMTQ_CAL_NT, this);
+  lp_var_t<uint8_t> actuationCalStatus = lp_var_t<uint8_t>(sid.objectId,
+      MGM_IMTQ_CAL_ACT_STATUS, this);
+private:
+
+};
+
+}
+
+#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */

mission/devices/IMTQHandler.cpp

@@ -331,9 +331,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
   localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
 
   /** Entries of calibrated MTM measurement dataset */
-  localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry);
   localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
 
   /** Entries of raw MTM measurement dataset */
@@ -749,14 +747,15 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
 
 void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
   PoolReadGuard rg(&calMtmMeasurementSet);
+  calMtmMeasurementSet.setValidity(true, true);
   int8_t offset = 2;
-  calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
+  calMtmMeasurementSet.mgmXyz[0] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                 *(packet + offset + 1) << 8 | *(packet + offset);
   offset += 4;
-  calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
+  calMtmMeasurementSet.mgmXyz[1] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                 *(packet + offset + 1) << 8 | *(packet + offset);
   offset += 4;
-  calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
+  calMtmMeasurementSet.mgmXyz[2] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                 *(packet + offset + 1) << 8 | *(packet + offset);
   offset += 4;
   calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) |
@@ -764,11 +763,11 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
                                              (*(packet + offset + 1) << 8) | (*(packet + offset));
   if (debugMode) {
 #if OBSW_VERBOSE_LEVEL >= 1
-    sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
+    sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mgmXyz[0] << " nT"
               << std::endl;
-    sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
+    sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mgmXyz[1] << " nT"
               << std::endl;
-    sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
+    sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mgmXyz[2] << " nT"
               << std::endl;
     sif::info << "IMTQ coil actuation status during MTM measurement: "
               << (unsigned int)calMtmMeasurementSet.coilActuationStatus.value << std::endl;

mission/devices/IMTQHandler.h

@@ -99,6 +99,8 @@ class IMTQHandler : public DeviceHandlerBase {
   IMTQ::PosZSelfTestSet posZselfTestDataset;
   IMTQ::NegZSelfTestSet negZselfTestDataset;
 
+  PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
+
   power::Switch_t switcher = power::NO_SWITCH;
 
   uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];

mission/devices/devicedefinitions/IMTQHandlerDefinitions.h

@@ -115,9 +115,7 @@ enum IMTQPoolIds : lp_id_t {
   COIL_Y_TEMPERATURE,
   COIL_Z_TEMPERATURE,
   MCU_TEMPERATURE,
-  MTM_CAL_X,
-  MTM_CAL_Y,
-  MTM_CAL_Z,
+  MGM_CAL_NT,
   ACTUATION_CAL_STATUS,
   MTM_RAW_X,
   MTM_RAW_Y,
@@ -408,9 +406,7 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
       : StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {}
 
   /** The unit of all measurements is nT */
-  lp_var_t<int32_t> mtmXnT = lp_var_t<int32_t>(sid.objectId, MTM_CAL_X, this);
-  lp_var_t<int32_t> mtmYnT = lp_var_t<int32_t>(sid.objectId, MTM_CAL_Y, this);
-  lp_var_t<int32_t> mtmZnT = lp_var_t<int32_t>(sid.objectId, MTM_CAL_Z, this);
+  lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT);
   /** 1 if coils were actuating during measurement otherwise 0 */
   lp_var_t<uint8_t> coilActuationStatus =
       lp_var_t<uint8_t>(sid.objectId, ACTUATION_CAL_STATUS, this);