Merge branch 'main' into spahr/SharedPowerLines

src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp

@@ -191,3 +191,5 @@ ReturnValue_t FreshDeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uni
 }
 
 datapool::SharedPool* FreshDeviceHandlerBase::getOptionalSharedPool() { return nullptr; }
+
+ModeHelper& FreshDeviceHandlerBase::getModeHelper() { return this->modeHelper; }

src/fsfw/devicehandlers/FreshDeviceHandlerBase.h

@@ -97,6 +97,11 @@ class FreshDeviceHandlerBase : public SystemObject,
   ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
   ModeTreeChildIF& getModeTreeChildIF() override;
 
+  /**
+   * @brief Return an interface to the ModeHelper.
+   */
+  ModeHelper& getModeHelper();
+
  protected:
   ActionHelper actionHelper;
   ModeHelper modeHelper;

src/fsfw/modes/ModeHelper.cpp

@@ -35,8 +35,12 @@ ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
       commandedMode = mode;
       commandedSubmode = submode;
 
-      if ((parentQueueId != MessageQueueIF::NO_QUEUE) &&
+      // if (((parentQueueId != MessageQueueIF::NO_QUEUE) &&
-          (theOneWhoCommandedAMode != parentQueueId)) {
+      //     (theOneWhoCommandedAMode != parentQueueId))) {
+      //   owner->setToExternalControl();
+      // }
+
+      if(theOneWhoCommandedAMode != parentQueueId and theOneWhoCommandedAMode != powerswitchQueueId) {
         owner->setToExternalControl();
       }
 
@@ -106,3 +110,7 @@ bool ModeHelper::isTimedOut() { return countdown.hasTimedOut(); }
 bool ModeHelper::isForced() { return forced; }
 
 void ModeHelper::setForced(bool forced) { this->forced = forced; }
+
+void ModeHelper::setPowerSwitchQueueId(MessageQueueId_t queueId) {
+  powerswitchQueueId = queueId;
+}

src/fsfw/modes/ModeHelper.h

@@ -21,10 +21,15 @@ class ModeHelper {
 
   /**
    * @param parentQueue the Queue id of the parent object.
-   * Set to 0 if no parent present
+   * Set to MessageQueueIF::NO_QUEUE if no parent present
    */
   void setParentQueue(MessageQueueId_t parentQueueId);
 
+  /**
+   * Set to MessageQueue::NO_QUEUE if no powerswitch is commanding the obejct.
+   */
+  void setPowerSwitchQueueId(MessageQueueId_t queueId);
+
   ReturnValue_t initialize(MessageQueueId_t parentQueueId);
 
   ReturnValue_t initialize(void);
@@ -42,6 +47,7 @@ class ModeHelper {
  protected:
   HasModesIF *owner;
   MessageQueueId_t parentQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t powerswitchQueueId = MessageQueueIF::NO_QUEUE;
 
   Countdown countdown;
 

src/fsfw/subsystem/SubsystemBase.cpp

@@ -64,6 +64,11 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
 
   for (; tableIter.value != nullptr; ++tableIter) {
     object_id_t object = tableIter.value->getObject();
+
+    // As default, the objectId in the commandTable is the same as the one in the childrenMap.
+    // The user has to specify otherwise if required.
+    object = commandObjectIdToChildrenMapObjectId(object);
+
     if ((iter = childrenMap.find(object)) == childrenMap.end()) {
       // illegal table entry, should only happen due to misconfigured mode table
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -119,7 +124,11 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
       continue;  // don't send redundant mode commands (produces event spam), but still command if
                  // mode is forced to reach lower levels
     }
-    ReturnValue_t result = commandQueue->sendMessage(iter->second.commandQueue, &command);
+    
+    // Get the messageQueueId if the receiver specified by the commandTable.
+    // This the same MessageQueueId as stored in the childrenMap if the commanded object is part of the childrenMap.
+    MessageQueueId_t commandedReceiver = ObjectManager::instance()->get<HasHealthIF>(tableIter->getObject())->getCommandQueue();
+    ReturnValue_t result = commandQueue->sendMessage(commandedReceiver, &command);
     if (result == returnvalue::OK) {
       ++commandsOutstanding;
     }
@@ -353,3 +362,7 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t childObjectId, MessageQue
   }
   return returnvalue::OK;
 }
+
+object_id_t SubsystemBase::commandObjectIdToChildrenMapObjectId(object_id_t commandId) {
+  return commandId;
+}

src/fsfw/subsystem/SubsystemBase.h

@@ -153,6 +153,15 @@ class SubsystemBase : public SystemObject,
   virtual void announceMode(bool recursive) override;
 
   virtual void modeChanged();
+
+  /**
+ * @brief This converts the objectId of the object we want to send a mode command to into the 
+ * objectId of the corresponding object in the childrenMap for the current mode command. 
+ * As default implementation, this is the same objectId, and this functions returns it's input value
+ * 
+ * It is up to the user to specify otherwise.
+ */
+  virtual object_id_t commandObjectIdToChildrenMapObjectId(object_id_t commandId);
 };
 
 #endif /* FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ */