Merge pull request 'preparing v1.43.2' (#575) from prep_v1.43.2 into develop

Reviewed-on: #575

CHANGELOG.md

@@ -16,6 +16,44 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v1.43.2] 2023-04-05
+
+## Changed
+
+- Adapted HK data rates to new table for LEOP SAFE mode.
+- GPS controller HK is now generated periodically as well.
+- Better mode combination checks for assembly components. This includes:
+  - IMTQ assembly
+  - Syrlinks assembly
+  - Dual Lane Assembly
+- RWs are no longer commanded by the ACS Controller during safe mode. Instead the RW speed command
+  is set to 0 as part or the `doShutDown` of the RW handler.
+
+## Fixed
+
+- Dual lane assemblies: Fix handling when health states are overwritten. Also add better handling
+  when some devices are permanent faulty and some are only faulty. In that case, only the faulty
+  devices will be restored.
+- ACS dual lane assembly: Gyro 3 helper mode was assigned to the Gyro 2 mode.
+
+# [v1.43.1] 2023-04-04
+
+## Fixed
+
+- Generic HK handling: Bug where HKs were generated a lot more often than required. This is the case
+  if a device handler `PERFORM_OPERATION` step is performed more than once per PST cycle.
+- Syrlinks now goes to `_MODE_TO_ON` when finishing the `doStartUp` transition.
+
+## Changed
+
+- Doubled GS PST interval instead of scheduling everything twice.
+- Syrlinks now only has one `PERFORM_OPERATION` step, but still has two communication steps.
+- PCDU components only allow setting `NEEDS_RECOVERY`, `HEALTHY` and `EXTERNAL_CONTROL` health
+  states now. TMP sensor components only allow `HEALTHY` , `EXTERNAL_CONTROL`, `FAULTY` and
+  `PERMANENT_FAULTY`.
+- TCS controller now does a sanity check on the temperature values: Values below -80 C or above
+  160 C are ignored.
+
 # [v1.43.0] 2023-04-04
 
 - q7s-package: v2.4.0

CMakeLists.txt

@@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 1)
 set(OBSW_VERSION_MINOR 43)
-set(OBSW_VERSION_REVISION 0)
+set(OBSW_VERSION_REVISION 2)
 
 # set(CMAKE_VERBOSE TRUE)
 

bsp_q7s/core/CoreController.cpp

@@ -138,7 +138,7 @@ ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &local
   localDataPoolMap.emplace(core::TEMPERATURE, &tempPoolEntry);
   localDataPoolMap.emplace(core::PS_VOLTAGE, &psVoltageEntry);
   localDataPoolMap.emplace(core::PL_VOLTAGE, &plVoltageEntry);
-  poolManager.subscribeForRegularPeriodicPacket({hkSet.getSid(), enableHkSet, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({hkSet.getSid(), enableHkSet, 60.0});
   return returnvalue::OK;
 }
 

bsp_q7s/core/ObjectFactory.cpp

@@ -26,7 +26,7 @@
 #include <mission/power/CspCookie.h>
 #include <mission/system/acs/ImtqAssembly.h>
 #include <mission/system/acs/StrAssembly.h>
-#include <mission/system/fdir/StrFdir.h>
+#include <mission/system/acs/StrFdir.h>
 #include <mission/system/objects/CamSwitcher.h>
 #include <mission/system/objects/SyrlinksAssembly.h>
 
@@ -62,9 +62,9 @@
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/com/SyrlinksFdir.h"
 #include "mission/system/com/comModeTree.h"
-#include "mission/system/fdir/GomspacePowerFdir.h"
 #include "mission/system/fdir/RtdFdir.h"
 #include "mission/system/objects/TcsBoardAssembly.h"
+#include "mission/system/power/GomspacePowerFdir.h"
 #include "mission/system/tree/payloadModeTree.h"
 #include "mission/system/tree/tcsModeTree.h"
 #include "mission/tmtc/tmFilters.h"
@@ -360,7 +360,8 @@ void ObjectFactory::createAcsBoardGpios(GpioCookie& cookie) {
 }
 
 void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF,
-                                             SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) {
+                                             SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher,
+                                             bool enableHkSets) {
   using namespace gpio;
   GpioCookie* gpioCookieAcsBoard = new GpioCookie();
   createAcsBoardGpios(*gpioCookieAcsBoard);
@@ -514,8 +515,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
 #endif
   RESET_ARGS_GNSS.gpioComIF = gpioComIF;
   RESET_ARGS_GNSS.waitPeriodMs = 100;
-  auto gpsCtrl =
-      new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, debugGps);
+  auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT,
+                                                enableHkSets, debugGps);
   gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS);
 
   ObjectFactory::createAcsBoardAssy(pwrSwitcher, assemblyChildren, gpsCtrl, gpioComIF);

bsp_q7s/core/ObjectFactory.h

@@ -62,7 +62,7 @@ void createTmpComponents();
 ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
 void createAcsBoardGpios(GpioCookie& cookie);
 void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
-                              PowerSwitchIF& pwrSwitcher);
+                              PowerSwitchIF& pwrSwitcher, bool enableHkSets);
 void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable,
                             HeaterHandler*& heaterHandler);
 void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets);

bsp_q7s/core/scheduling.cpp

@@ -538,7 +538,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 
   FixedTimeslotTaskIF* gomSpacePstTask =
       factory.createFixedTimeslotTask("GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4,
-                                      0.5, missedDeadlineFunc, &RR_SCHEDULING);
+                                      0.25, missedDeadlineFunc, &RR_SCHEDULING);
   result = pst::pstGompaceCan(gomSpacePstTask);
   if (result != returnvalue::OK) {
     if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {

bsp_q7s/fmObjectFactory.cpp

@@ -55,7 +55,7 @@ void ObjectFactory::produce(void* args) {
 #endif
 
 #if OBSW_ADD_ACS_BOARD == 1
-  createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher);
+  createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true);
 #endif
   HeaterHandler* heaterHandler;
   createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);

linux/acs/GpsHyperionLinuxController.cpp

@@ -18,8 +18,11 @@
 #include <ctime>
 
 GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
-                                                       bool debugHyperionGps)
-    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {}
+                                                       bool enableHkSets, bool debugHyperionGps)
+    : ExtendedControllerBase(objectId),
+      gpsSet(this),
+      enableHkSets(enableHkSets),
+      debugHyperionGps(debugHyperionGps) {}
 
 GpsHyperionLinuxController::~GpsHyperionLinuxController() {
   gps_stream(&gps, WATCH_DISABLE, nullptr);
@@ -86,7 +89,7 @@ ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
   localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
   localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
   localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
-  poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), false, 30.0});
+  poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), enableHkSets, 30.0});
   return returnvalue::OK;
 }
 

linux/acs/GpsHyperionLinuxController.h

@@ -29,7 +29,7 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
 
   enum ReadModes { SHM = 0, SOCKET = 1 };
 
-  GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
+  GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool enableHkSets,
                              bool debugHyperionGps = false);
   virtual ~GpsHyperionLinuxController();
 
@@ -58,6 +58,7 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
  private:
   GpsPrimaryDataset gpsSet;
   gps_data_t gps = {};
+  bool enableHkSets = false;
   const char* currentClientBuf = nullptr;
   ReadModes readMode = ReadModes::SOCKET;
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);

mission/acs/ImtqHandler.cpp

@@ -795,9 +795,9 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
   localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
 
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 12.0));
+      subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 12.0));
+      subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0));
   poolManager.subscribeForDiagPeriodicPacket(

mission/acs/RwHandler.cpp

@@ -55,6 +55,10 @@ void RwHandler::doShutDown() {
     PoolReadGuard pg(&tmDataset);
     tmDataset.setValidity(false, true);
   }
+  {
+    PoolReadGuard pg(&rwSpeedActuationSet);
+    rwSpeedActuationSet.setRwSpeed(0, 10);
+  }
   // The power switch is handled by the assembly, so we can go off here directly.
   setMode(MODE_OFF);
 }

mission/acs/str/StarTrackerHandler.cpp

@@ -277,7 +277,7 @@ void StarTrackerHandler::performOperationHook() {
   }
 }
 
-Submode_t StarTrackerHandler::getInitialSubmode() { return SUBMODE_BOOTLOADER; }
+Submode_t StarTrackerHandler::getInitialSubmode() { return startracker::SUBMODE_BOOTLOADER; }
 
 ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
   if (strHelperHandlingSpecialRequest) {
@@ -705,7 +705,7 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t
         return INVALID_SUBMODE;
       }
     case MODE_ON:
-      if (submode == SUBMODE_BOOTLOADER || submode == SUBMODE_FIRMWARE) {
+      if (submode == startracker::SUBMODE_BOOTLOADER || submode == startracker::SUBMODE_FIRMWARE) {
         return returnvalue::OK;
       } else {
         return INVALID_SUBMODE;
@@ -736,6 +736,7 @@ void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
 }
 
 void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) {
+  using namespace startracker;
   uint8_t dhbSubmode = getSubmode();
   // We hide that the transition to submode firmware actually goes through the submode bootloader.
   // This is because the startracker always starts in bootloader mode but we want to allow direct
@@ -762,6 +763,7 @@ void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) {
 }
 
 void StarTrackerHandler::doNormalTransition(Mode_t modeFrom, Submode_t subModeFrom) {
+  using namespace startracker;
   if (subModeFrom == SUBMODE_FIRMWARE) {
     setMode(MODE_NORMAL);
   } else if (subModeFrom == SUBMODE_BOOTLOADER) {
@@ -787,7 +789,7 @@ void StarTrackerHandler::bootFirmware(Mode_t toMode) {
       if (toMode == MODE_NORMAL) {
         setMode(toMode, 0);
       } else {
-        setMode(toMode, SUBMODE_FIRMWARE);
+        setMode(toMode, startracker::SUBMODE_FIRMWARE);
       }
       sif::info << "STR: Firmware boot success" << std::endl;
       internalState = InternalState::IDLE;

mission/acs/str/StarTrackerHandler.h

@@ -54,9 +54,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   Submode_t getInitialSubmode() override;
 
-  static const Submode_t SUBMODE_BOOTLOADER = 1;
-  static const Submode_t SUBMODE_FIRMWARE = 2;
-
  protected:
   void doStartUp() override;
   void doShutDown() override;

mission/acs/str/strHelpers.h

@@ -11,6 +11,9 @@
 
 namespace startracker {
 
+static const Submode_t SUBMODE_BOOTLOADER = 1;
+static const Submode_t SUBMODE_FIRMWARE = 2;
+
 /**
  * @brief    Returns the frame type field of a decoded frame.
  */

mission/com/SyrlinksHandler.cpp

@@ -21,15 +21,15 @@ SyrlinksHandler::~SyrlinksHandler() = default;
 
 void SyrlinksHandler::doStartUp() {
   if (internalState == InternalState::OFF) {
-    transitionCommandPending = false;
     internalState = InternalState::ENABLE_TEMPERATURE_PROTECTION;
     commandExecuted = false;
   }
   if (internalState == InternalState::ENABLE_TEMPERATURE_PROTECTION) {
     if (commandExecuted) {
       // Go to normal mode immediately and disable transmitter on startup.
-      setMode(_MODE_TO_NORMAL);
+      setMode(_MODE_TO_ON);
       internalState = InternalState::IDLE;
+      transState = TransitionState::IDLE;
       commandExecuted = false;
     }
   }
@@ -37,15 +37,16 @@ void SyrlinksHandler::doStartUp() {
 
 void SyrlinksHandler::doShutDown() {
   // In any case, always disable TX first.
-  if (internalState != InternalState::SET_TX_STANDBY) {
-    internalState = InternalState::SET_TX_STANDBY;
-    transitionCommandPending = false;
+  if (internalState != InternalState::TX_TRANSITION) {
+    internalState = InternalState::TX_TRANSITION;
+    transState = TransitionState::SET_TX_STANDBY;
     commandExecuted = false;
   }
-  if (internalState == InternalState::SET_TX_STANDBY) {
+  if (internalState == InternalState::TX_TRANSITION) {
     if (commandExecuted) {
       temperatureSet.setValidity(false, true);
       internalState = InternalState::OFF;
+      transState = TransitionState::IDLE;
       commandExecuted = false;
       setMode(_MODE_POWER_DOWN);
     }
@@ -99,30 +100,43 @@ ReturnValue_t SyrlinksHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
 }
 
 ReturnValue_t SyrlinksHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if (transState == TransitionState::CMD_PENDING or transState == TransitionState::DONE) {
+    return NOTHING_TO_SEND;
+  }
   switch (internalState) {
     case InternalState::ENABLE_TEMPERATURE_PROTECTION: {
       *id = syrlinks::WRITE_LCL_CONFIG;
+      transState = TransitionState::CMD_PENDING;
       return buildCommandFromCommand(*id, nullptr, 0);
     }
-    case InternalState::SET_TX_MODULATION: {
-      *id = syrlinks::SET_TX_MODE_MODULATION;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    }
-    case InternalState::SELECT_MODULATION_BPSK: {
-      *id = syrlinks::SET_WAVEFORM_BPSK;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    }
-    case InternalState::SELECT_MODULATION_0QPSK: {
-      *id = syrlinks::SET_WAVEFORM_0QPSK;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    }
-    case InternalState::SET_TX_CW: {
-      *id = syrlinks::SET_TX_MODE_CW;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    }
-    case InternalState::SET_TX_STANDBY: {
-      *id = syrlinks::SET_TX_MODE_STANDBY;
-      return buildCommandFromCommand(*id, nullptr, 0);
+    case InternalState::TX_TRANSITION: {
+      switch (transState) {
+        case TransitionState::SET_TX_MODULATION: {
+          *id = syrlinks::SET_TX_MODE_MODULATION;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        case TransitionState::SELECT_MODULATION_BPSK: {
+          *id = syrlinks::SET_WAVEFORM_BPSK;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        case TransitionState::SELECT_MODULATION_0QPSK: {
+          *id = syrlinks::SET_WAVEFORM_0QPSK;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        case TransitionState::SET_TX_CW: {
+          *id = syrlinks::SET_TX_MODE_CW;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        case TransitionState::SET_TX_STANDBY: {
+          *id = syrlinks::SET_TX_MODE_STANDBY;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        default: {
+          return NOTHING_TO_SEND;
+        }
+      }
+      transState = TransitionState::CMD_PENDING;
+      break;
     }
     default: {
       break;
@@ -622,8 +636,6 @@ void SyrlinksHandler::parseAgcHighByte(const uint8_t* packet) {
   agcValueHighByte = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
 }
 
-void SyrlinksHandler::setNormalDatapoolEntriesInvalid() {}
-
 uint32_t SyrlinksHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 2500; }
 
 ReturnValue_t SyrlinksHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -652,11 +664,11 @@ ReturnValue_t SyrlinksHandler::initializeLocalDataPool(localpool::DataPool& loca
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(rxDataset.getSid(), enableHkSets, 60.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(temperatureSet.getSid(), enableHkSets, 20.0));
+      subdp::RegularHkPeriodicParams(temperatureSet.getSid(), enableHkSets, 120.0));
   return returnvalue::OK;
 }
 
-void SyrlinksHandler::setModeNormal() { setMode(MODE_NORMAL); }
+void SyrlinksHandler::setModeNormal() { setMode(_MODE_TO_NORMAL); }
 
 float SyrlinksHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67.87; }
 
@@ -675,12 +687,31 @@ ReturnValue_t SyrlinksHandler::handleAckReply(const uint8_t* packet) {
       break;
     }
     case (syrlinks::SET_WAVEFORM_BPSK):
-    case (syrlinks::SET_WAVEFORM_0QPSK):
-    case (syrlinks::SET_TX_MODE_STANDBY):
-    case (syrlinks::SET_TX_MODE_MODULATION):
+    case (syrlinks::SET_WAVEFORM_0QPSK): {
+      if (result == returnvalue::OK and isTransitionalMode()) {
+        transState = TransitionState::SET_TX_MODULATION;
+        commandExecuted = true;
+      }
+      break;
+    }
+    case (syrlinks::SET_TX_MODE_STANDBY): {
+      if (result == returnvalue::OK and isTransitionalMode()) {
+        transState = TransitionState::DONE;
+        commandExecuted = true;
+      }
+      break;
+    }
+    case (syrlinks::SET_TX_MODE_MODULATION): {
+      if (result == returnvalue::OK and isTransitionalMode()) {
+        transState = TransitionState::DONE;
+        commandExecuted = true;
+      }
+      break;
+    }
     case (syrlinks::SET_TX_MODE_CW): {
       if (result == returnvalue::OK and isTransitionalMode()) {
         commandExecuted = true;
+        transState = TransitionState::DONE;
       }
       break;
     }
@@ -704,7 +735,7 @@ ReturnValue_t SyrlinksHandler::handleAckReply(const uint8_t* packet) {
 
 ReturnValue_t SyrlinksHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   if (mode == HasModesIF::MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL) {
-    if (submode >= com::Submode::NUM_SUBMODES) {
+    if (submode >= com::Submode::NUM_SUBMODES or submode < com::Submode::RX_ONLY) {
       return HasModesIF::INVALID_SUBMODE;
     }
     return returnvalue::OK;
@@ -731,72 +762,54 @@ void SyrlinksHandler::setDebugMode(bool enable) { this->debugMode = enable; }
 
 void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
   Mode_t tgtMode = getBaseMode(getMode());
-  auto commandDone = [&]() {
-    setMode(tgtMode);
-    transitionCommandPending = false;
+  auto doneHandler = [&]() {
     internalState = InternalState::IDLE;
+    transState = TransitionState::IDLE;
+    DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
   };
-  auto txOnHandler = [&](InternalState selMod) {
-    if (internalState == InternalState::IDLE) {
-      transitionCommandPending = false;
-      commandExecuted = false;
-      internalState = selMod;
-    }
-    // Select modulation first (BPSK or 0QPSK).
-    if (internalState == selMod) {
-      if (commandExecuted) {
-        transitionCommandPending = false;
-        internalState = InternalState::SET_TX_MODULATION;
-        commandExecuted = false;
-      }
-    }
-    // Now go into modulation mode.
-    if (internalState == InternalState::SET_TX_MODULATION) {
-      if (commandExecuted) {
-        commandDone();
-        return true;
-      }
-    }
-    return false;
-  };
+  if (transState == TransitionState::DONE) {
+    return doneHandler();
+  }
   auto txStandbyHandler = [&]() {
-    if (internalState == InternalState::IDLE) {
-      transitionCommandPending = false;
-      internalState = InternalState::SET_TX_STANDBY;
-      commandExecuted = false;
-    }
-    if (internalState == InternalState::SET_TX_STANDBY) {
-      if (commandExecuted) {
-        commandDone();
-        return;
-      }
-    }
+    txDataset.setReportingEnabled(false);
+    poolManager.changeCollectionInterval(temperatureSet.getSid(), 60.0);
+    transState = TransitionState::SET_TX_STANDBY;
+    internalState = InternalState::TX_TRANSITION;
   };
+  auto txOnHandler = [&](TransitionState tgtTransitionState) {
+    txDataset.setReportingEnabled(true);
+    poolManager.changeCollectionInterval(txDataset.getSid(), 10.0);
+    poolManager.changeCollectionInterval(temperatureSet.getSid(), 5.0);
+    transState = tgtTransitionState;
+    internalState = InternalState::TX_TRANSITION;
+  };
+
   if (tgtMode == HasModesIF::MODE_ON or tgtMode == DeviceHandlerIF::MODE_NORMAL) {
+    // If submode has not changed, no special transition handling necessary.
+    if (getSubmode() == subModeFrom) {
+      return doneHandler();
+    }
+    // Transition is on-going, wait for it to finish.
+    if (transState != TransitionState::IDLE) {
+      return;
+    }
+    // Transition start logic.
     switch (getSubmode()) {
       case (com::Submode::RX_AND_TX_DEFAULT_DATARATE): {
         auto currentDatarate = com::getCurrentDatarate();
         if (currentDatarate == com::Datarate::LOW_RATE_MODULATION_BPSK) {
-          if (txOnHandler(InternalState::SELECT_MODULATION_BPSK)) {
-            return;
-          }
+          txOnHandler(TransitionState::SELECT_MODULATION_BPSK);
         } else if (currentDatarate == com::Datarate::HIGH_RATE_MODULATION_0QPSK) {
-          if (txOnHandler(InternalState::SELECT_MODULATION_0QPSK)) {
-            return;
-          }
+          txOnHandler(TransitionState::SELECT_MODULATION_0QPSK);
         }
         break;
       }
       case (com::Submode::RX_AND_TX_LOW_DATARATE): {
-        if (txOnHandler(InternalState::SELECT_MODULATION_BPSK)) {
-          return;
-        }
+        txOnHandler(TransitionState::SELECT_MODULATION_BPSK);
         break;
       }
       case (com::Submode::RX_AND_TX_HIGH_DATARATE): {
-        if (txOnHandler(InternalState::SELECT_MODULATION_0QPSK)) {
-          return;
-        }
+        txOnHandler(TransitionState::SELECT_MODULATION_0QPSK);
         break;
       }
       case (com::Submode::RX_ONLY): {
@@ -804,21 +817,17 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
         return;
       }
       case (com::Submode::RX_AND_TX_CW): {
-        if (internalState == InternalState::IDLE) {
-          internalState = InternalState::SET_TX_STANDBY;
-          commandExecuted = false;
-        }
-        if (commandExecuted) {
-          commandDone();
-          return;
-        }
+        txOnHandler(TransitionState::SET_TX_CW);
         break;
       }
       default: {
-        commandDone();
+        sif::error << "SyrlinksHandler: Unexpected submode " << getSubmode() << std::endl;
+        DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
       }
     }
   } else if (tgtMode == HasModesIF::MODE_OFF) {
     txStandbyHandler();
+  } else {
+    return doneHandler();
   }
 }

mission/com/SyrlinksHandler.h

@@ -46,7 +46,6 @@ class SyrlinksHandler : public DeviceHandlerBase {
                              size_t* foundLen) override;
   ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
   ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;
-  void setNormalDatapoolEntriesInvalid() override;
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                         LocalDataPoolManager& poolManager) override;
@@ -112,22 +111,26 @@ class SyrlinksHandler : public DeviceHandlerBase {
   float tempPowerAmplifier = 0;
   float tempBasebandBoard = 0;
   bool commandExecuted = false;
-  bool transitionCommandPending = false;
 
   uint8_t commandBuffer[syrlinks::MAX_COMMAND_SIZE];
 
   enum class InternalState {
     OFF,
     ENABLE_TEMPERATURE_PROTECTION,
+    TX_TRANSITION,
+    IDLE
+  } internalState = InternalState::OFF;
+
+  enum class TransitionState {
+    IDLE,
     SELECT_MODULATION_BPSK,
     SELECT_MODULATION_0QPSK,
     SET_TX_MODULATION,
     SET_TX_CW,
     SET_TX_STANDBY,
-    IDLE
-  };
-
-  InternalState internalState = InternalState::OFF;
+    CMD_PENDING,
+    DONE
+  } transState = TransitionState::IDLE;
 
   /**
    * This object is used to store the id of the next command to execute. This controls the

mission/controller/AcsController.cpp

@@ -221,9 +221,8 @@ void AcsController::performSafe() {
 
   updateCtrlValData(errAng);
   updateActuatorCmdData(cmdDipolMtqs);
-  // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
-  //                  acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
-  //                  acsParameters.rwHandlingParameters.rampTime);
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquerParameter.torqueDuration);
 }
 
 void AcsController::performDetumble() {
@@ -472,6 +471,18 @@ void AcsController::performPointingCtrl() {
   //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
+ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
+                                              uint16_t dipoleTorqueDuration) {
+  {
+    PoolReadGuard pg(&dipoleSet);
+    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
+                  torquer::LOCK_CTX);
+    torquer::NEW_ACTUATION_FLAG = true;
+    dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
+  }
+  return returnvalue::OK;
+}
+
 ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                               uint16_t dipoleTorqueDuration, int32_t rw1Speed,
                                               int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
@@ -565,7 +576,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
-  poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0});
+  poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
   // MGM Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
@@ -575,7 +586,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
   localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
-  poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.0});
   // SUS Raw
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
@@ -589,7 +600,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
-  poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0});
+  poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
   // SUS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
@@ -606,43 +617,43 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
-  poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.0});
   // GYR Raw
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
-  poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 5.0});
+  poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.0});
   // GYR Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
-  poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
   // GPS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
-  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
   // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
-  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
-  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.0});
   // Actuator CMD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
   localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
-  poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 12.0});
+  poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
   return returnvalue::OK;
 }
 

mission/controller/AcsController.h

@@ -105,6 +105,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void modeChanged(Mode_t mode, Submode_t submode);
   void announceMode(bool recursive);
 
+  ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
+                                 uint16_t dipoleTorqueDuration);
   ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                  uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
                                  int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);

mission/controller/ThermalController.cpp

@@ -172,7 +172,7 @@ void ThermalController::performControlOperation() {
         transitionToOffCycles++;
         // if heater still ON after 10 cycles, switch OFF again
         if (transitionToOffCycles == 10) {
-          for (uint8_t i; i < heater::Switchers::NUMBER_OF_SWITCHES; i++) {
+          for (uint8_t i = 0; i < heater::Switchers::NUMBER_OF_SWITCHES; i++) {
             heaterHandler.switchHeater(static_cast<heater::Switchers>(i),
                                        HeaterHandler::SwitchState::OFF);
           }
@@ -260,13 +260,13 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo
   enableHkSets = true;
 #endif
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), enableHkSets, 60.0));
+      subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), enableHkSets, 120.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(susTemperatures.getSid(), enableHkSets, 60.0));
+      subdp::RegularHkPeriodicParams(susTemperatures.getSid(), enableHkSets, 240.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), enableHkSets, 60.0));
+      subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), enableHkSets, 120.0));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), enableHkSets, 30.0));
+      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), enableHkSets, 120.0));
 
   return returnvalue::OK;
 }
@@ -988,13 +988,15 @@ void ThermalController::ctrlAcsBoard() {
   sensors[4].first = sensorTemperatures.tcsBoard.isValid();
   sensors[4].second = sensorTemperatures.tcsBoard.value;
   numSensors = 5;
-  if (selectAndReadSensorTemp()) {
-    if (chooseHeater(switchNr, redSwitchNr)) {
-      HeaterContext htrCtx(switchNr, redSwitchNr, acsBoardLimits);
-      checkLimitsAndCtrlHeater(htrCtx);
+  {
+    HeaterContext htrCtx(switchNr, redSwitchNr, acsBoardLimits);
+    if (selectAndReadSensorTemp(htrCtx)) {
+      if (chooseHeater(switchNr, redSwitchNr)) {
+        checkLimitsAndCtrlHeater(htrCtx);
+      }
+      resetSensorsArray();
+      return;
     }
-    resetSensorsArray();
-    return;
   }
   resetSensorsArray();
   // B side
@@ -1007,15 +1009,18 @@ void ThermalController::ctrlAcsBoard() {
   sensors[3].first = sensorTemperatures.tcsBoard.isValid();
   sensors[3].second = sensorTemperatures.tcsBoard.value;
   numSensors = 4;
-  if (selectAndReadSensorTemp()) {
-    if (chooseHeater(switchNr, redSwitchNr)) {
-      HeaterContext htrCtx(switchNr, redSwitchNr, acsBoardLimits);
-      checkLimitsAndCtrlHeater(htrCtx);
-    }
-  } else {
-    if (chooseHeater(switchNr, redSwitchNr)) {
-      if (heaterHandler.getSwitchState(switchNr)) {
-        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+
+  {
+    HeaterContext htrCtx(switchNr, redSwitchNr, acsBoardLimits);
+    if (selectAndReadSensorTemp(htrCtx)) {
+      if (chooseHeater(switchNr, redSwitchNr)) {
+        checkLimitsAndCtrlHeater(htrCtx);
+      }
+    } else {
+      if (chooseHeater(switchNr, redSwitchNr)) {
+        if (heaterHandler.getSwitchState(switchNr)) {
+          heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
+        }
       }
     }
   }
@@ -1546,7 +1551,7 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate
   heaterTransitionControl(heaterSwitchStates);
 }
 void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) {
-  if (selectAndReadSensorTemp()) {
+  if (selectAndReadSensorTemp(htrCtx)) {
     if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) {
       checkLimitsAndCtrlHeater(htrCtx);
     }
@@ -1560,9 +1565,11 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) {
   }
   resetSensorsArray();
 }
-bool ThermalController::selectAndReadSensorTemp() {
+bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) {
   for (unsigned i = 0; i < numSensors; i++) {
-    if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE) {
+    if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE and
+        sensors[i].second > SANITY_LIMIT_LOWER_TEMP and
+        sensors[i].second < SANITY_LIMIT_UPPER_TEMP) {
       sensorTemp = sensors[i].second;
       thermalStates[thermalComponent].errorCounter = 0;
       return true;
@@ -1745,7 +1752,7 @@ void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object,
 void ThermalController::startTransition(Mode_t mode_, Submode_t submode_) {
   triggerEvent(CHANGING_MODE, mode_, submode_);
   if (mode_ == MODE_OFF) {
-    for (uint8_t i; i < heater::Switchers::NUMBER_OF_SWITCHES; i++) {
+    for (uint8_t i = 0; i < heater::Switchers::NUMBER_OF_SWITCHES; i++) {
       heaterHandler.switchHeater(static_cast<heater::Switchers>(i),
                                  HeaterHandler::SwitchState::OFF);
     }

mission/controller/ThermalController.h

@@ -91,6 +91,8 @@ class ThermalController : public ExtendedControllerBase {
  public:
   static const uint16_t INVALID_TEMPERATURE = 999;
   static const uint8_t NUMBER_OF_SENSORS = 16;
+  static constexpr int16_t SANITY_LIMIT_LOWER_TEMP = -80;
+  static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160;
 
   ThermalController(object_id_t objectId, HeaterHandler& heater);
 
@@ -299,7 +301,7 @@ class ThermalController : public ExtendedControllerBase {
   void heaterCtrlTempTooHighHandler(HeaterContext& heaterContext, const char* whatLimit);
 
   bool chooseHeater(heater::Switchers& switchNr, heater::Switchers redSwitchNr);
-  bool selectAndReadSensorTemp();
+  bool selectAndReadSensorTemp(HeaterContext& htrCtx);
 
   void ctrlAcsBoard();
   void ctrlMgt();

mission/pollingSeqTables.cpp

@@ -26,8 +26,7 @@ ReturnValue_t pst::pstSyrlinks(FixedTimeslotTaskIF *thisSequence) {
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4,
-                        DeviceHandlerIF::PERFORM_OPERATION);
+
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::SEND_WRITE);
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
   thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.75, DeviceHandlerIF::SEND_READ);
@@ -114,40 +113,15 @@ ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
   thisSequence->addSlot(objects::PDU2_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
   thisSequence->addSlot(objects::ACU_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
 
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.5, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::SEND_READ);
 
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
-
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.5, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::PERFORM_OPERATION);
-
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.5, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::SEND_WRITE);
-
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.5, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_WRITE);
-
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.75, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.75, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.75, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.75, DeviceHandlerIF::SEND_READ);
-
-  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.75, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.75, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.75, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.75, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
   if (thisSequence->checkSequence() != returnvalue::OK) {
     sif::error << "GomSpace PST initialization failed" << std::endl;
     return returnvalue::FAILED;

mission/power/AcuHandler.cpp

@@ -149,9 +149,9 @@ ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localData
   localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
 
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 20.0));
+      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 3000.0));
+      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0));
   return returnvalue::OK;
 }
 

mission/power/GomspaceDeviceHandler.cpp

@@ -642,3 +642,11 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU
   }
   return returnvalue::OK;
 }
+
+ReturnValue_t GomspaceDeviceHandler::setHealth(HealthState health) {
+  if (health != HealthState::HEALTHY and health != HealthState::EXTERNAL_CONTROL and
+      health != HealthState::NEEDS_RECOVERY) {
+    return returnvalue::FAILED;
+  }
+  return returnvalue::OK;
+}

mission/power/GomspaceDeviceHandler.h

@@ -80,6 +80,9 @@ class GomspaceDeviceHandler : public DeviceHandlerBase {
   ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
   void setNormalDatapoolEntriesInvalid() override;
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
+
+  ReturnValue_t setHealth(HealthState health) override;
+
   /**
    * @brief   The command to generate a request to receive the full housekeeping table is device
    *          specific. Thus the child has to build this command.

mission/power/P60DockHandler.cpp

@@ -166,9 +166,9 @@ ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &local
   localDataPoolMap.emplace(pool::P60DOCK_ANT6_DEPL, new PoolEntry<int8_t>({0}));
   localDataPoolMap.emplace(pool::P60DOCK_AR6_DEPL, new PoolEntry<int8_t>({0}));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 20.0));
+      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 3000.0));
+      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0));
   return returnvalue::OK;
 }
 

mission/power/Pdu1Handler.cpp

@@ -90,9 +90,9 @@ ReturnValue_t Pdu1Handler::initializeLocalDataPool(localpool::DataPool &localDat
                                                    LocalDataPoolManager &poolManager) {
   initializePduPool(localDataPoolMap, poolManager, pcdu::INIT_SWITCHES_PDU1);
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 20.0));
+      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 3000.0));
+      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0));
   return returnvalue::OK;
 }
 

mission/power/Pdu2Handler.cpp

@@ -49,9 +49,9 @@ ReturnValue_t Pdu2Handler::initializeLocalDataPool(localpool::DataPool &localDat
                                                    LocalDataPoolManager &poolManager) {
   initializePduPool(localDataPoolMap, poolManager, pcdu::INIT_SWITCHES_PDU2);
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 10.0));
+      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 30.0));
+      subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0));
   return returnvalue::OK;
 }
 

mission/system/CMakeLists.txt

@@ -3,5 +3,6 @@ add_subdirectory(tree)
 add_subdirectory(acs)
 add_subdirectory(com)
 add_subdirectory(fdir)
+add_subdirectory(power)
 
 target_sources(${LIB_EIVE_MISSION} PRIVATE DualLanePowerStateMachine.cpp)

mission/system/acs/AcsBoardAssembly.cpp

@@ -57,8 +57,8 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
   modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
   if (recoveryState == RecoveryState::RECOVERY_IDLE) {
     result = checkAndHandleHealthStates(mode, submode);
-    if (result == NEED_TO_CHANGE_HEALTH) {
-      return returnvalue::OK;
+    if (result != returnvalue::OK) {
+      return result;
     }
   }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
@@ -238,7 +238,7 @@ void AcsBoardAssembly::refreshHelperModes() {
     helper.gyro0SideAMode = childrenMap.at(helper.gyro0AdisIdSideA).mode;
     helper.gyro1SideAMode = childrenMap.at(helper.gyro1L3gIdSideA).mode;
     helper.gyro2SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
-    helper.gyro3SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
+    helper.gyro3SideBMode = childrenMap.at(helper.gyro3L3gIdSideB).mode;
     helper.mgm0SideAMode = childrenMap.at(helper.mgm0Lis3IdSideA).mode;
     helper.mgm1SideAMode = childrenMap.at(helper.mgm1Rm3100IdSideA).mode;
     helper.mgm2SideBMode = childrenMap.at(helper.mgm2Lis3IdSideB).mode;
@@ -256,37 +256,98 @@ ReturnValue_t AcsBoardAssembly::initialize() {
   return AssemblyBase::initialize();
 }
 
-ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
-                                                           Submode_t deviceSubmode) {
+ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t commandedMode,
+                                                           Submode_t commandedSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
+  bool healthNeedsToBeOverwritten = false;
   auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
                                       object_id_t o3) {
     HealthState h0 = healthHelper.healthTable->getHealth(o0);
     HealthState h1 = healthHelper.healthTable->getHealth(o1);
     HealthState h2 = healthHelper.healthTable->getHealth(o2);
     HealthState h3 = healthHelper.healthTable->getHealth(o3);
+    // All device are faulty or permanent faulty, but this is a safe mode assembly, so we need
+    // to restore the devices.
     if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and
         (h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) {
-      overwriteDeviceHealth(o0, h0);
-      overwriteDeviceHealth(o1, h1);
-      overwriteDeviceHealth(o2, h2);
-      overwriteDeviceHealth(o3, h3);
+      uint8_t numPermFaulty = 0;
+      if (h0 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (h1 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (h2 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (h3 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (numPermFaulty < 4) {
+        // Some are faulty and some are permanent faulty, so only set faulty ones to
+        // EXTERNAL_CONTROL.
+        if (h0 == FAULTY) {
+          overwriteDeviceHealth(o0, h0);
+        }
+        if (h1 == FAULTY) {
+          overwriteDeviceHealth(o1, h1);
+        }
+        if (h2 == FAULTY) {
+          overwriteDeviceHealth(o2, h2);
+        }
+        if (h3 == FAULTY) {
+          overwriteDeviceHealth(o3, h3);
+        }
+      } else {
+        // All permanent faulty, so set all to EXTERNAL_CONTROL
+        overwriteDeviceHealth(o0, h0);
+        overwriteDeviceHealth(o1, h1);
+        overwriteDeviceHealth(o2, h2);
+        overwriteDeviceHealth(o3, h3);
+      }
+      healthNeedsToBeOverwritten = true;
     }
     if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
         h3 == EXTERNAL_CONTROL) {
       modeHelper.setForced(true);
     }
   };
-  if (healthHelper.healthTable->getHealth(helper.gpsId) == EXTERNAL_CONTROL) {
+  if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == EXTERNAL_CONTROL or
+      healthHelper.healthTable->getHealth(helper.healthDevGps1) == EXTERNAL_CONTROL) {
     modeHelper.setForced(true);
   }
-  if (deviceSubmode == duallane::DUAL_MODE) {
+  if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY and
+      healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) {
+    overwriteDeviceHealth(helper.healthDevGps1, FAULTY);
+    healthNeedsToBeOverwritten = true;
+  } else if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY and
+             healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY) {
+    overwriteDeviceHealth(helper.healthDevGps0, FAULTY);
+  } else if (healthHelper.healthTable->isFaulty(helper.healthDevGps0) and
+             healthHelper.healthTable->isFaulty(helper.healthDevGps1)) {
+    overwriteDeviceHealth(helper.healthDevGps0,
+                          healthHelper.healthTable->getHealth(helper.healthDevGps0));
+    overwriteDeviceHealth(helper.healthDevGps1,
+                          healthHelper.healthTable->getHealth(helper.healthDevGps1));
+    healthNeedsToBeOverwritten = true;
+  }
+
+  if (commandedSubmode == duallane::DUAL_MODE) {
     checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
                              helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
     checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
+
                              helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
   }
+  if (healthNeedsToBeOverwritten) {
+    // If we are overwriting the health states, we are already in a transition to dual mode,
+    // and we would like that transition to complete. The default behaviour is to go back to the
+    // old mode. We force our behaviour by overwriting the internal modes.
+    mode = commandedMode;
+    submode = commandedSubmode;
+    return NEED_TO_CHANGE_HEALTH;
+  }
   return status;
 }
 

mission/system/acs/CMakeLists.txt

@@ -9,4 +9,5 @@ target_sources(
           SusAssembly.cpp
           AcsBoardFdir.cpp
           acsModeTree.cpp
-          SusFdir.cpp)
+          SusFdir.cpp
+          StrFdir.cpp)

mission/system/acs/DualLaneAssemblyBase.cpp

@@ -36,6 +36,8 @@ void DualLaneAssemblyBase::performChildOperation() {
 void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   pwrStateMachine.reset();
+  dualToSingleSideTransition = false;
+  sideSwitchState = SideSwitchState::NONE;
 
   if (mode != MODE_OFF) {
     // Special exception: A transition from dual side to single mode must be handled like
@@ -112,8 +114,11 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
 
 ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   using namespace duallane;
-  if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
-    return returnvalue::FAILED;
+  if (mode != MODE_OFF and (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE)) {
+    return HasModesIF::INVALID_SUBMODE;
+  }
+  if (mode == MODE_OFF and submode != SUBMODE_NONE) {
+    return HasModesIF::INVALID_SUBMODE;
   }
   return returnvalue::OK;
 }

mission/system/acs/DualLaneAssemblyBase.h

@@ -70,7 +70,8 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
    * @param submode
    */
   virtual ReturnValue_t pwrStateMachineWrapper();
-  virtual ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+
   /**
    * Custom recovery implementation to ensure that the power lines are commanded off for a
    * recovery.

mission/system/acs/ImtqAssembly.cpp

@@ -18,7 +18,7 @@ ReturnValue_t ImtqAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   if (recoveryState == RECOVERY_IDLE) {
     ReturnValue_t result = checkAndHandleHealthState(mode, submode);
     if (result == NEED_TO_CHANGE_HEALTH) {
-      return OK;
+      return result;
     }
   }
   HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
@@ -35,9 +35,12 @@ ReturnValue_t ImtqAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wa
 
 ReturnValue_t ImtqAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
+    if (submode != SUBMODE_NONE) {
+      return HasModesIF::INVALID_SUBMODE;
+    }
     return returnvalue::OK;
   }
-  return returnvalue::FAILED;
+  return HasModesIF::INVALID_MODE;
 }
 
 ReturnValue_t ImtqAssembly::checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode) {

mission/system/acs/StrAssembly.cpp

@@ -2,6 +2,8 @@
 
 #include <eive/objects.h>
 
+#include "mission/acs/str/strHelpers.h"
+
 StrAssembly::StrAssembly(object_id_t objectId) : AssemblyBase(objectId) {
   ModeListEntry entry;
   entry.setObject(objects::STAR_TRACKER);
@@ -31,5 +33,12 @@ ReturnValue_t StrAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan
 }
 
 ReturnValue_t StrAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if ((mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) and submode != SUBMODE_NONE) {
+    return HasModesIF::INVALID_SUBMODE;
+  }
+  if (mode == MODE_ON and
+      (submode != startracker::SUBMODE_BOOTLOADER and submode != startracker::SUBMODE_FIRMWARE)) {
+    return HasModesIF::INVALID_SUBMODE;
+  }
   return returnvalue::OK;
 }

mission/system/acs/SusAssembly.cpp

@@ -26,8 +26,8 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   }
   if (recoveryState == RecoveryState::RECOVERY_IDLE) {
     result = checkAndHandleHealthStates(mode, submode);
-    if (result == NEED_TO_CHANGE_HEALTH) {
-      return returnvalue::OK;
+    if (result != returnvalue::OK) {
+      return result;
     }
   }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
@@ -47,26 +47,9 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod
   bool needsSecondStep = false;
   handleSideSwitchStates(submode, needsSecondStep);
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
-    if (mode == devMode) {
+    if (isModeCommandable(objectId, devMode)) {
       modeTable[tableIdx].setMode(mode);
-    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
-      if (isUseable(objectId, devMode)) {
-        if (devMode == MODE_ON) {
-          modeTable[tableIdx].setMode(mode);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-        } else {
-          modeTable[tableIdx].setMode(MODE_ON);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-          if (internalState != STATE_SECOND_STEP) {
-            needsSecondStep = true;
-          }
-        }
-      }
-    } else if (mode == MODE_ON) {
-      if (isUseable(objectId, devMode)) {
-        modeTable[tableIdx].setMode(MODE_ON);
-        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-      }
+      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
     }
   };
   switch (submode) {
@@ -134,38 +117,31 @@ ReturnValue_t SusAssembly::initialize() {
   return AssemblyBase::initialize();
 }
 
-bool SusAssembly::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;
-}
-
 void SusAssembly::refreshHelperModes() {
   for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
     helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
   }
 }
 
-ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
+ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t commandedMode,
+                                                      Submode_t commandedSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
+  bool needsHealthOverwritten = false;
   auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
     HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
     HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
-    if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
-        (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
+    if (healthNom == PERMANENT_FAULTY and healthRed == FAULTY) {
+      overwriteDeviceHealth(devRed, healthRed);
+      needsHealthOverwritten = true;
+    } else if (healthNom == FAULTY and healthRed == PERMANENT_FAULTY) {
+      overwriteDeviceHealth(devNom, healthNom);
+      needsHealthOverwritten = true;
+    } else if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
+               (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
       overwriteDeviceHealth(devNom, healthNom);
       overwriteDeviceHealth(devRed, healthRed);
+      needsHealthOverwritten = true;
     }
   };
   auto checkHealthForOneDev = [&](object_id_t dev) {
@@ -174,7 +150,7 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
       modeHelper.setForced(true);
     }
   };
-  if (deviceSubmode == duallane::DUAL_MODE) {
+  if (commandedSubmode == duallane::DUAL_MODE) {
     uint8_t idx = 0;
     for (idx = 0; idx < 6; idx++) {
       checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
@@ -184,5 +160,12 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
       checkHealthForOneDev(helper.susIds[idx]);
     }
   }
+  if (needsHealthOverwritten) {
+    mode = commandedMode;
+    submode = commandedSubmode;
+    // We need second step instead of NEED_TO_CHANGE_HEALTH because we do not want recovery
+    // handling.
+    return NEED_TO_CHANGE_HEALTH;
+  }
   return status;
 }

mission/system/acs/SusAssembly.h

@@ -56,13 +56,6 @@ class SusAssembly : public DualLaneAssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
 
-  /**
-   * 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);
   void powerStateMachine(Mode_t mode, Submode_t submode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void refreshHelperModes();

mission/system/com/SyrlinksAssembly.cpp

@@ -1,6 +1,7 @@
 #include "SyrlinksAssembly.h"
 
 #include <eive/objects.h>
+#include <mission/com/defs.h>
 
 using namespace returnvalue;
 
@@ -19,7 +20,7 @@ ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode)
   if (recoveryState == RECOVERY_IDLE) {
     ReturnValue_t result = checkAndHandleHealthState(mode, submode);
     if (result == NEED_TO_CHANGE_HEALTH) {
-      return OK;
+      return result;
     }
   }
   executeTable(iter);
@@ -34,10 +35,16 @@ ReturnValue_t SyrlinksAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
 }
 
 ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
-  if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
+  if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL) {
+    if (submode >= com::Submode::NUM_SUBMODES or submode < com::Submode::RX_ONLY) {
+      return HasModesIF::INVALID_SUBMODE;
+    }
     return returnvalue::OK;
   }
-  return returnvalue::FAILED;
+  if (mode == MODE_OFF and submode != SUBMODE_NONE) {
+    return HasModesIF::INVALID_SUBMODE;
+  }
+  return returnvalue::OK;
 }
 
 ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode,

mission/system/fdir/CMakeLists.txt

@@ -1,2 +1 @@
-target_sources(${LIB_EIVE_MISSION} PRIVATE RtdFdir.cpp StrFdir.cpp
-                                           GomspacePowerFdir.cpp)
+target_sources(${LIB_EIVE_MISSION} PRIVATE RtdFdir.cpp)

mission/system/objects/PowerStateMachineBase.cpp

@@ -1,5 +1,7 @@
 #include "PowerStateMachineBase.h"
 
+#include "fsfw/serviceinterface.h"
+
 PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout)
     : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}
 

mission/system/power/CMakeLists.txt

@@ -0,0 +1 @@
+target_sources(${LIB_EIVE_MISSION} PRIVATE GomspacePowerFdir.cpp)

mission/tcs/Tmp1075Handler.cpp

@@ -132,3 +132,11 @@ ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &local
 }
 
 void Tmp1075Handler::setModeNormal() { setMode(_MODE_TO_NORMAL); }
+
+ReturnValue_t Tmp1075Handler::setHealth(HealthState health) {
+  if (health != FAULTY and health != PERMANENT_FAULTY and health != HEALTHY and
+      health != EXTERNAL_CONTROL) {
+    return returnvalue::FAILED;
+  }
+  return returnvalue::OK;
+}

mission/tcs/Tmp1075Handler.h

@@ -37,6 +37,7 @@ class Tmp1075Handler : public DeviceHandlerBase {
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                         LocalDataPoolManager &poolManager) override;
+  ReturnValue_t setHealth(HealthState health) override;
 
  private:
   /**