continued RW Assembly

linux/devices/GPSHyperionLinuxController.cpp

@@ -110,12 +110,12 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() {
   // The data from the device will generally be read all at once. Therefore, we
   // can set all field here
   if (not myGpsmm.is_open()) {
-    if(gpsNotOpenSwitch) {
+    if (gpsNotOpenSwitch) {
       // Opening failed
-  #if FSFW_VERBOSE_LEVEL >= 1
-      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed | " <<
-          "Error " << errno << " | " << gps_errstr(errno) << std::endl;
-  #endif
+#if FSFW_VERBOSE_LEVEL >= 1
+      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed | "
+                   << "Error " << errno << " | " << gps_errstr(errno) << std::endl;
+#endif
 
       gpsNotOpenSwitch = false;
     }
@@ -124,9 +124,10 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() {
   gps_data_t *gps = nullptr;
   gps = myGpsmm.read();
   if (gps == nullptr) {
-    if(gpsReadFailedSwitch) {
+    if (gpsReadFailedSwitch) {
       gpsReadFailedSwitch = false;
-      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl;
+      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed"
+                   << std::endl;
     }
     return;
   }

linux/devices/devicedefinitions/PlocMPSoCDefinitions.h

@@ -643,24 +643,24 @@ class TcModeIdle : public TcBase {
 };
 
 class TcCamcmdSend : public TcBase {
-public:
+ public:
   TcCamcmdSend(uint16_t sequenceCount) : TcBase(apid::TC_CAM_CMD_SEND, sequenceCount) {}
 
-protected:
-    ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) {
-      if (commandDataLen > MAX_DATA_LENGTH) {
-        return INVALID_LENGTH;
-      }
-      std::memcpy(this->getPacketData(), commandData, commandDataLen);
-      *(this->getPacketData() + commandDataLen) = CARRIAGE_RETURN;
-      uint16_t trueLength = commandDataLen + sizeof(CARRIAGE_RETURN) + CRC_SIZE;
-      this->setPacketDataLength(trueLength - 1);
-      return HasReturnvaluesIF::RETURN_OK;
+ protected:
+  ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) {
+    if (commandDataLen > MAX_DATA_LENGTH) {
+      return INVALID_LENGTH;
     }
-private:
-    static const uint8_t MAX_DATA_LENGTH = 10;
-    static const uint8_t CARRIAGE_RETURN = 0xD;
+    std::memcpy(this->getPacketData(), commandData, commandDataLen);
+    *(this->getPacketData() + commandDataLen) = CARRIAGE_RETURN;
+    uint16_t trueLength = commandDataLen + sizeof(CARRIAGE_RETURN) + CRC_SIZE;
+    this->setPacketDataLength(trueLength - 1);
+    return HasReturnvaluesIF::RETURN_OK;
+  }
 
+ private:
+  static const uint8_t MAX_DATA_LENGTH = 10;
+  static const uint8_t CARRIAGE_RETURN = 0xD;
 };
 
 }  // namespace mpsoc

linux/devices/ploc/PlocMPSoCHandler.cpp

@@ -255,7 +255,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() {
   this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT);
   this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT);
   this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT);
-   this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, SpacePacket::PACKET_MAX_SIZE);
+  this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, SpacePacket::PACKET_MAX_SIZE);
 }
 
 ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize,
@@ -653,8 +653,9 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) {
     sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl;
   }
   const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE;
-  std::string camCmdRptMsg(reinterpret_cast<const char*>(
-      dataFieldPtr), tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - 3);
+  std::string camCmdRptMsg(
+      reinterpret_cast<const char*>(dataFieldPtr),
+      tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - 3);
   uint8_t ackValue = *(packet.getPacketData() + packet.getPacketDataLength() - 2);
   sif::info << "CamCmdRpt message: " << camCmdRptMsg << std::endl;
   sif::info << "CamCmdRpt Ack value: 0x" << std::hex << static_cast<unsigned int>(ackValue)

linux/devices/ploc/PlocMPSoCHandler.h

@@ -4,11 +4,11 @@
 #include <string>
 
 #include "PlocMPSoCHelper.h"
-#include "fsfw/tmtcpacket/SpacePacket.h"
 #include "fsfw/action/CommandActionHelper.h"
 #include "fsfw/action/CommandsActionsIF.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/ipc/QueueFactory.h"
+#include "fsfw/tmtcpacket/SpacePacket.h"
 #include "fsfw/tmtcservices/SourceSequenceCounter.h"
 #include "fsfw_hal/linux/gpio/Gpio.h"
 #include "fsfw_hal/linux/uart/UartComIF.h"

mission/system/RwAssembly.cpp

@@ -2,6 +2,186 @@
 
 RwAssembly::RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                        power::Switch_t switcher, RwHelper helper)
-  : AssemblyBase(objectId, parentId), helper(helper) {
-
+    : AssemblyBase(objectId, parentId), helper(helper), switcher(pwrSwitcher, switcher) {
+  ModeListEntry entry;
+  for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
+    entry.setObject(helper.rwIds[idx]);
+    entry.setMode(MODE_OFF);
+    entry.setSubmode(SUBMODE_NONE);
+    entry.setInheritSubmode(false);
+    modeTable.insert(entry);
+  }
+}
+
+void RwAssembly::performChildOperation() {
+  auto state = switcher.getState();
+  if (state != PowerSwitcher::WAIT_OFF and state != PowerSwitcher::WAIT_ON) {
+    AssemblyBase::performChildOperation();
+    return;
+  }
+  switcher.doStateMachine();
+  if (state == PowerSwitcher::WAIT_OFF and switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
+    // Indicator that a transition to off is finished
+    AssemblyBase::handleModeReached();
+  } else if (state == PowerSwitcher::WAIT_ON and
+             switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
+    // Indicator that mode commanding can be performed now
+    AssemblyBase::startTransition(targetMode, targetSubmode);
+  }
+}
+
+ReturnValue_t RwAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = RETURN_OK;
+  // Initialize the mode table to ensure all devices are in a defined state
+  for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
+    modeTable[idx].setMode(MODE_OFF);
+    modeTable[idx].setSubmode(SUBMODE_NONE);
+  }
+  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+      result = handleNormalOrOnModeCmd(mode, submode);
+    }
+  }
+  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
+  executeTable(tableIter);
+  return result;
+}
+
+ReturnValue_t RwAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  int devsInCorrectMode = 0;
+  try {
+    for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
+      if (childrenMap.at(helper.rwIds[idx]).mode != wantedMode) {
+        devsInCorrectMode++;
+      }
+    }
+  } catch (const std::out_of_range& e) {
+    sif::error << "RwAssembly: Invalid children map: " << e.what() << std::endl;
+  }
+  if (devsInCorrectMode < 3) {
+    if (warningSwitch) {
+      sif::warning << "RwAssembly::checkChildrenStateOn: Only " << devsInCorrectMode
+                   << " devices in correct mode" << std::endl;
+      warningSwitch = false;
+    }
+    return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+  }
+  return RETURN_OK;
+}
+
+ReturnValue_t RwAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) {
+    return RETURN_OK;
+  }
+  return HasModesIF::INVALID_MODE;
+}
+
+void RwAssembly::startTransition(Mode_t mode, Submode_t submode) {
+  if (mode != MODE_OFF) {
+    switcher.turnOn(true);
+    switcher.doStateMachine();
+    if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
+      AssemblyBase::startTransition(mode, submode);
+    } else {
+      // Need to wait with mode commanding until power switcher is done
+      targetMode = mode;
+      targetSubmode = submode;
+    }
+  } else {
+    // Perform regular mode commanding first
+    AssemblyBase::startTransition(mode, submode);
+  }
+}
+
+void RwAssembly::handleModeReached() {
+  if (targetMode == MODE_OFF) {
+    switcher.turnOff(true);
+    switcher.doStateMachine();
+    // Need to wait with call to AssemblyBase::handleModeReached until power switcher is done
+    if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
+      AssemblyBase::handleModeReached();
+    }
+  } else {
+    AssemblyBase::handleModeReached();
+  }
+}
+
+void RwAssembly::handleChildrenLostMode(ReturnValue_t result) {
+  AssemblyBase::handleChildrenLostMode(result);
+}
+
+ReturnValue_t RwAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = RETURN_OK;
+  bool needsSecondStep = false;
+  Mode_t devMode = 0;
+  object_id_t objId = 0;
+  try {
+    for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
+      devMode = childrenMap.at(helper.rwIds[idx]).mode;
+      objId = helper.rwIds[idx];
+      if (mode == devMode) {
+        modeTable[idx].setMode(mode);
+      } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+        if (isUseable(objId, devMode)) {
+          if (devMode == MODE_ON) {
+            modeTable[idx].setMode(mode);
+            modeTable[idx].setSubmode(SUBMODE_NONE);
+          } else {
+            modeTable[idx].setMode(MODE_ON);
+            modeTable[idx].setSubmode(SUBMODE_NONE);
+            if (internalState != STATE_SECOND_STEP) {
+              needsSecondStep = true;
+            }
+          }
+        }
+      } else if (mode == MODE_ON) {
+        if (isUseable(objId, devMode)) {
+          modeTable[idx].setMode(MODE_ON);
+          modeTable[idx].setSubmode(SUBMODE_NONE);
+        }
+      }
+    }
+  } catch (const std::out_of_range& e) {
+    sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
+  }
+  if (needsSecondStep) {
+    result = NEED_SECOND_STEP;
+  }
+  return result;
+}
+
+bool RwAssembly::isUseable(object_id_t object, Mode_t mode) {
+  if (healthHelper.healthTable->isFaulty(object)) {
+    return false;
+  }
+
+  // Check if device is already in target mode
+  if (childrenMap[object].mode == mode) {
+    return true;
+  }
+
+  if (healthHelper.healthTable->isCommandable(object)) {
+    return true;
+  }
+  return false;
+}
+
+ReturnValue_t RwAssembly::initialize() {
+  ReturnValue_t result = RETURN_OK;
+  for (const auto& obj : helper.rwIds) {
+    result = registerChild(obj);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+  }
+  return SubsystemBase::initialize();
+}
+
+void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) {
+  if (targetMode == MODE_OFF) {
+    AssemblyBase::handleModeTransitionFailed(result);
+  } else {
+    // To avoid transitioning back to off
+    triggerEvent(MODE_TRANSITION_FAILED, result);
+  }
 }

mission/system/RwAssembly.h

@@ -2,6 +2,7 @@
 #define MISSION_SYSTEM_RWASS_H_
 
 #include <fsfw/devicehandlers/AssemblyBase.h>
+#include <fsfw/power/PowerSwitcher.h>
 
 struct RwHelper {
   RwHelper(std::array<object_id_t, 4> rwIds) : rwIds(rwIds) {}
@@ -9,13 +10,41 @@ struct RwHelper {
   std::array<object_id_t, 4> rwIds = {};
 };
 
-class RwAssembly: public AssemblyBase {
-public:
+class RwAssembly : public AssemblyBase {
+ public:
   RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
-                   power::Switch_t switcher, RwHelper helper);
-private:
+             power::Switch_t switcher, RwHelper helper);
+
+ private:
+  static constexpr uint8_t NUMBER_RWS = 4;
   RwHelper helper;
-  PowerSwitcher pwrSwitcher;
+  PowerSwitcher switcher;
+  bool warningSwitch = true;
+  FixedArrayList<ModeListEntry, NUMBER_RWS> modeTable;
+
+  ReturnValue_t initialize() override;
+
+  ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
+  /**
+   * Check whether it makes sense to send mode commands to the device
+   * @param object
+   * @param mode
+   * @return
+   */
+  bool isUseable(object_id_t object, Mode_t mode);
+
+  // AssemblyBase implementation
+  void performChildOperation() override;
+  ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
+  ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
+  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+  void startTransition(Mode_t mode, Submode_t submode) override;
+  void handleModeReached() override;
+
+  // These two overrides prevent a transition of the whole assembly back to off just because
+  // some devices are not working
+  void handleChildrenLostMode(ReturnValue_t result) override;
+  void handleModeTransitionFailed(ReturnValue_t result) override;
 };
 
 #endif /* MISSION_SYSTEM_RWASS_H_ */

mission/system/TcsBoardAssembly.cpp

@@ -32,7 +32,6 @@ void TcsBoardAssembly::performChildOperation() {
              switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
     // Indicator that mode commanding can be performed now
     AssemblyBase::startTransition(targetMode, targetSubmode);
-    // AssemblyBase::performChildOperation();
   }
 }
 

mission/system/TcsBoardAssembly.h

@@ -21,8 +21,6 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF {
 
   ReturnValue_t initialize() override;
 
-  void performChildOperation() override;
-
  private:
   static constexpr uint8_t NUMBER_RTDS = 16;
   PowerSwitcher switcher;
@@ -44,6 +42,7 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF {
   MessageQueueId_t getEventReceptionQueue() override;
 
   // AssemblyBase implementation
+  void performChildOperation() override;
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;