added ReceiversParameterMessagesIF and ParameterHelper to ACS Ctrl

mission/controller/AcsController.cpp

@@ -14,6 +14,7 @@ AcsController::AcsController(object_id_t objectId)
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
       detumbleCounter{0},
+      parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
       susDataRaw(this),
@@ -26,6 +27,23 @@ AcsController::AcsController(object_id_t objectId)
       actuatorCmdData(this) {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
+  ReturnValue_t result = actionHelper.handleActionMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  result = parameterHelper.handleParameterMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  return result;
+}
+
+MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
+
+ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
+                                          ParameterWrapper* parameterWrapper,
+                                          const ParameterWrapper* newValues,
+                                          uint16_t startAtIndex) {
   return returnvalue::OK;
 }
 
@@ -53,6 +71,7 @@ void AcsController::performControlOperation() {
             break;
           case SUBMODE_IDLE:
           case SUBMODE_PTG_TARGET:
+          case SUBMODE_PTG_TARGET_GS:
           case SUBMODE_PTG_NADIR:
           case SUBMODE_PTG_INERTIAL:
             performPointingCtrl();
@@ -105,17 +124,19 @@ void AcsController::performSafe() {
     bool magMomMtqValid = false;
     if (validMekf == returnvalue::OK) {
       safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
-                      mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
+                        mgmDataProcessed.magIgrfModel.value,
-                      susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
+                        mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value,
-                      mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
+                        susDataProcessed.isValid(), mekfData.satRotRateMekf.value,
-                      sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+                        mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng,
+                        magMomMtq, &magMomMtqValid);
     } else {
       safeCtrl.safeNoMekf(
           now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
-        susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
+          susDataProcessed.susVecTotDerivative.value,
-        mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
-        mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
+          mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value,
-        sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+          mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng,
+          magMomMtq, &magMomMtqValid);
     }
 
     double dipolCmdUnits[3] = {0, 0, 0};
@@ -170,7 +191,8 @@ void AcsController::performSafe() {
     //    PoolReadGuard pg(&dipoleSet);
     //    MutexGuard mg(torquer::lazyLock());
     //    torquer::NEW_ACTUATION_FLAG = true;
-  //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
+    //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
+    //    torqueDuration);
     //  }
   }
 
@@ -187,9 +209,9 @@ void AcsController::performDetumble() {
                        &mekfData, &validMekf);
 
     double magMomMtq[3] = {0, 0, 0};
-  detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
+    detumble.bDotLaw(
-                   mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
+        mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
-                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
+        mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
     double dipolCmdUnits[3] = {0, 0, 0};
     actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
 
@@ -253,10 +275,13 @@ void AcsController::performPointingCtrl() {
                             targetQuat, refSatRate);
         break;
       case SUBMODE_PTG_TARGET:
-      guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
+        guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
-                                      refSatRate);
+                                        targetQuat, refSatRate);
+        break;
+      case SUBMODE_PTG_TARGET_GS:
+        guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
+                                 now, targetQuat, refSatRate);
         break;
-
       case SUBMODE_PTG_NADIR:
         guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
                                        refSatRate);
@@ -288,7 +313,8 @@ void AcsController::performPointingCtrl() {
       ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
     }
 
-  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
+    double cmdSpeedRws[4] = {0, 0, 0,
+                             0};  // Should be given to the actuator reaction wheel as input
     actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
                               &(sensorValues.rw2Set.currSpeed.value),
                               &(sensorValues.rw3Set.currSpeed.value),
@@ -460,10 +486,6 @@ 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() {
     ACS::SensorValues sensorValues;
     {
@@ -505,7 +527,8 @@ void AcsController::copyMgmData() {
                     3 * sizeof(float));
         mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
         mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
-      mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
+        mgmDataRaw.actuationCalStatus.setValid(
+            sensorValues.imtqMgmSet.coilActuationStatus.isValid());
       }
     }
   }

mission/controller/AcsController.h

@@ -3,6 +3,8 @@
 
 #include <fsfw/controller/ExtendedControllerBase.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/parameters/ParameterHelper.h>
+#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
 
 #include "acs/ActuatorCmd.h"
 #include "acs/Guidance.h"
@@ -18,7 +20,7 @@
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
 #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
-class AcsController : public ExtendedControllerBase {
+class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
  public:
   static constexpr dur_millis_t INIT_DELAY = 500;
 
@@ -36,6 +38,12 @@ class AcsController : public ExtendedControllerBase {
   static const Event SAFE_RATE_VIOLATION =
       MAKE_EVENT(0, severity::MEDIUM);  //!< The limits for the rotation in safe mode were violated.
 
+  MessageQueueId_t getCommandQueue() const;
+
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
+                             ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+                             uint16_t startAtIndex) override;
+
  protected:
   void performSafe();
   void performDetumble();
@@ -54,6 +62,8 @@ class AcsController : public ExtendedControllerBase {
 
   uint8_t detumbleCounter;
 
+  ParameterHelper parameterHelper;
+
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
   InternalState internalState = InternalState::STARTUP;
@@ -68,8 +78,6 @@ 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);
 
   /* ACS Datasets */
   IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);