Merge pull request 'prep v8.2.0' (#907) from prep-release into main

Reviewed-on: #907

CHANGELOG.md

@@ -16,6 +16,57 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v8.2.0] 2024-06-26
+
+## Changed
+
+- STR quaternions are now used by the `MEKF` by default
+- Changed nominal `SUS Assembly` side to `B Side`.
+- Changed source for state machine of detumbling to SUS and MGM only.
+- Changed `FusedRotRateData` dataset to always display rotation rate from SUS and MGM.
+- Solution from `QUEST` will be set to invalid if sun vector and magnetic field vector are too close
+  to each other.
+- Changed collection intervals of dataset collection
+  - `GPS Controller`: `GPS Set` to 60s
+  - `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s
+  - `RW Handler`: `Status Set` to 30s
+  - `STR Handler`: `Solution Set` to 30s
+  - `ACS Controller`: `MGM Sensor`, `MGM Processed`, `SUS Sensor`, `SUS Processed`, `GYR Sensor`,
+    `GPS Processed` to 60s
+  - `ACS Controller` at or below `IDLE`: `CTRL Values`, `ACT Commands`, `Attitude Estimation`,
+    `Fused Rotation Rate` to 30s
+  - `ACS Controller` above `IDLE`: `CTRL Values`, `ACT Commands`, `Attitude Estimation`,
+    `Fused Rotation Rate` to 10s
+
+## Fixed
+
+- Added null termination for PLOC MPSoC image taking command which could possibly lead to
+  default target filenames.
+
+# [v8.1.1] 2024-06-05
+
+## Added
+
+- PLOC SUPV MPSoC update re-try logic for the `WRITE_MEMORY` command. These packets form  > 98%
+  of all packets required for a software update, but the update mechanism is not tolerant against
+  occasional glitches on the RS485 communication to the PLOC SUPV. A simple re-try mechanism which
+  tries to re-attempt packet handling up to three times for those packets is introduced.
+
+# [v8.1.0] 2024-05-29
+
+## Fixed
+
+- Small fix for transition failure handling of the MPSoC when the `START_MPSOC` action command
+  to the supervisor fails.
+- Fixed inits of arrays within the `MEKF` not being zeros.
+- Important bugfix for PLOC SUPV: The SUPV previously was able to steal packets from the special
+  communication helper, for example during software updates.
+- Corrected sigma of STR for `MEKF`.
+
+## Added
+
+- Added new command to cancel the PLOC SUPV special communication helper.
+
 # [v8.0.0] 2024-05-13
 
 - `eive-tmtc` v7.0.0

CMakeLists.txt

@@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 8)
-set(OBSW_VERSION_MINOR 0)
+set(OBSW_VERSION_MINOR 2)
 set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)

linux/acs/GpsHyperionLinuxController.cpp

@@ -100,7 +100,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(), enableHkSets, 30.0});
+  poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), enableHkSets, 60.0});
   localDataPoolMap.emplace(GpsHyperion::SKYVIEW_UNIX_SECONDS, new PoolEntry<double>());
   localDataPoolMap.emplace(GpsHyperion::PRN_ID, new PoolEntry<int16_t>());
   localDataPoolMap.emplace(GpsHyperion::AZIMUTH, new PoolEntry<int16_t>());

linux/payload/FreshMpsocHandler.cpp

@@ -456,7 +456,7 @@ void FreshMpsocHandler::handleActionCommandFailure(ActionId_t actionId, ReturnVa
       if (actionId != supv::START_MPSOC) {
         return;
       }
-      sif::info << "PlocMPSoCHandler::handleActionCommandFailure: MPSoC boot command failed"
+      sif::info << "FreshMpsocHandler::handleActionCommandFailure: MPSoC boot command failed"
                 << std::endl;
       // This is commonly the case when the MPSoC is already operational. Thus the power state is
       // set to on here
@@ -1224,6 +1224,7 @@ bool FreshMpsocHandler::handleHwStartup() {
   if (powerState == PowerState::SUPV_FAILED) {
     setMode(MODE_OFF);
     powerState = PowerState::IDLE;
+    transitionState = TransitionState::NONE;
     return false;
   }
   if (powerState == PowerState::PENDING_STARTUP) {

linux/payload/FreshSupvHandler.cpp

@@ -241,6 +241,10 @@ ReturnValue_t FreshSupvHandler::executeAction(ActionId_t actionId, MessageQueueI
       uartManager->initiateUpdateContinuation();
       return EXECUTION_FINISHED;
     }
+    case ABORT_LONGER_REQUEST: {
+      uartManager->stop();
+      return EXECUTION_FINISHED;
+    }
     case MEMORY_CHECK_WITH_FILE: {
       UpdateParams params;
       result = extractBaseParams(&data, size, params);
@@ -849,6 +853,10 @@ ReturnValue_t FreshSupvHandler::prepareWipeMramCmd(const uint8_t* commandData, s
 ReturnValue_t FreshSupvHandler::parseTmPackets() {
   uint8_t* receivedData = nullptr;
   size_t receivedSize = 0;
+  // We do not want to steal packets from the long request handler.
+  if (uartManager->longerRequestActive()) {
+    return returnvalue::OK;
+  }
   while (true) {
     ReturnValue_t result =
         uartManager->readReceivedMessage(comCookie, &receivedData, &receivedSize);

linux/payload/PlocSupvUartMan.cpp

@@ -11,6 +11,8 @@
 #include <fstream>
 
 #include "OBSWConfig.h"
+#include "fsfw/returnvalues/returnvalue.h"
+#include "linux/payload/plocSupvDefs.h"
 #include "tas/hdlc.h"
 #ifdef XIPHOS_Q7S
 #include "bsp_q7s/fs/FilesystemHelper.h"
@@ -21,9 +23,13 @@
 
 #include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/timemanager/Countdown.h"
+
+#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
 #include "mission/utility/Filenaming.h"
 #include "mission/utility/ProgressPrinter.h"
 #include "mission/utility/Timestamp.h"
+#endif
+
 #include "tas/crc.h"
 
 using namespace returnvalue;
@@ -277,23 +283,6 @@ ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() {
   return returnvalue::OK;
 }
 
-// ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) {
-// #ifdef XIPHOS_Q7S
-//   ReturnValue_t result = FilesystemHelper::checkPath(path);
-//   if (result != returnvalue::OK) {
-//     return result;
-//   }
-// #endif
-//   if (not std::filesystem::exists(path)) {
-//     return PATH_NOT_EXISTS;
-//   }
-//   eventBufferReq.path = path;
-//   request = Request::REQUEST_EVENT_BUFFER;
-//   //uartComIF->flushUartTxAndRxBuf(comCookie);
-//   semaphore->release();
-//   return returnvalue::OK;
-// }
-
 void PlocSupvUartManager::stop() {
   MutexGuard mg(lock);
   if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) {
@@ -449,10 +438,8 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() {
                    update.bytesWritten);
       return result;
     }
-    result = handlePacketTransmissionNoReply(packet, 5000);
+    result = writeMemoryHandlingWithRetryLogic(packet, progPercent);
     if (result != returnvalue::OK) {
-      triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount),
-                   update.bytesWritten);
       return result;
     }
 
@@ -463,7 +450,25 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() {
 #if OBSW_DEBUG_PLOC_SUPERVISOR == 1
     progressPrinter.print(update.bytesWritten);
 #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
-    // TaskFactory::delayTask(1);
+  }
+  return result;
+}
+
+ReturnValue_t PlocSupvUartManager::writeMemoryHandlingWithRetryLogic(supv::WriteMemory& packet,
+                                                                     unsigned progPercent) {
+  ReturnValue_t result = returnvalue::OK;
+  // Simple re-try logic in place to deal with communication unreliability in orbit.
+  for (uint8_t retryCount = 0; retryCount < MAX_RETRY_COUNT; retryCount++) {
+    result = handlePacketTransmissionNoReply(packet, COM_TIMEOUT_MS);
+    if (result == returnvalue::OK) {
+      return result;
+    }
+    triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount),
+                 update.bytesWritten);
+    // Clear data structures related to reply handling.
+    serial::flushTxRxBuf(serialPort);
+    recRingBuf.clear();
+    decodedRingBuf.clear();
   }
   return result;
 }
@@ -572,7 +577,16 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply(
   bool ackReceived = false;
   bool packetWasHandled = false;
   while (true) {
-    handleUartReception();
+    ReturnValue_t status = handleUartReception();
+    if (status != returnvalue::OK) {
+      result = status;
+      if (result == HDLC_ERROR) {
+        // We could bail here immediately.. but I prefer to wait for the timeout, because we should
+        // ensure that all packets which might be related to the transfer are still received and
+        // cleared from all data structures related to reply handling.
+        // return result;
+      }
+    }
     if (not decodedQueue.empty()) {
       size_t packetLen = 0;
       decodedQueue.retrieve(&packetLen);
@@ -615,7 +629,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply(
       return result::NO_REPLY_TIMEOUT;
     }
   }
-  return returnvalue::OK;
+  return result;
 }
 
 int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, size_t packetLen) {

linux/payload/PlocSupvUartMan.h

@@ -118,6 +118,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
   static constexpr Event HDLC_FRAME_REMOVAL_ERROR = MAKE_EVENT(31, severity::INFO);
   static constexpr Event HDLC_CRC_ERROR = MAKE_EVENT(32, severity::INFO);
 
+  static constexpr unsigned MAX_RETRY_COUNT = 3;
   PlocSupvUartManager(object_id_t objectId);
   virtual ~PlocSupvUartManager();
   /**
@@ -199,6 +200,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
   static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_END = returnvalue::makeCode(1, 4);
   static constexpr ReturnValue_t HDLC_ERROR = returnvalue::makeCode(1, 5);
 
+  static constexpr uint32_t COM_TIMEOUT_MS = 3000;
+
   static const uint16_t CRC16_INIT = 0xFFFF;
   // Event buffer reply will carry 24 space packets with 1016 bytes and one space packet with
   // 192 bytes
@@ -369,6 +372,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
    */
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
 
+  ReturnValue_t writeMemoryHandlingWithRetryLogic(supv::WriteMemory& packet, unsigned progPercent);
+
   void performUartShutdown();
   void updateVtime(uint8_t vtime);
 };

linux/payload/plocMpsocHelpers.h

@@ -920,13 +920,14 @@ class TcCamTakePic : public TcBase {
     }
     size_t deserLen = commandDataLen;
     size_t serLen = 0;
-    fileName = reinterpret_cast<const char*>(commandData);
+    fileName = std::string(reinterpret_cast<const char*>(commandData));
     if (fileName.size() > MAX_FILENAME_SIZE) {
       return FILENAME_TOO_LONG;
     }
     deserLen -= fileName.length() + 1;
     *dataPtr += fileName.length() + 1;
     uint8_t** payloadPtr = &payloadStart;
+    memset(payloadStart, 0, FILENAME_FIELD_SIZE);
     memcpy(payloadStart, fileName.data(), fileName.size());
     *payloadPtr += FILENAME_FIELD_SIZE;
     serLen += FILENAME_FIELD_SIZE;

linux/payload/plocSupvDefs.h

@@ -159,6 +159,7 @@ static const DeviceCommandId_t ENABLE_NVMS = 59;
 static const DeviceCommandId_t CONTINUE_UPDATE = 60;
 static const DeviceCommandId_t MEMORY_CHECK_WITH_FILE = 61;
 static constexpr DeviceCommandId_t MEMORY_CHECK = 62;
+static constexpr DeviceCommandId_t ABORT_LONGER_REQUEST = 63;
 
 /** Reply IDs */
 enum ReplyId : DeviceCommandId_t {
@@ -1145,14 +1146,14 @@ class WriteMemory : public TcBase {
       : TcBase(params, Apid::MEM_MAN, static_cast<uint8_t>(tc::MemManId::WRITE), 1) {}
 
   ReturnValue_t buildPacket(ccsds::SequenceFlags seqFlags, uint16_t sequenceCount, uint8_t memoryId,
-                            uint32_t startAddress, uint16_t length, uint8_t* updateData) {
+                            uint32_t currentAddr, uint16_t length, uint8_t* updateData) {
     if (length > CHUNK_MAX) {
       sif::error << "WriteMemory::WriteMemory: Invalid length" << std::endl;
       return SerializeIF::BUFFER_TOO_SHORT;
     }
     spParams.creator.setSeqFlags(seqFlags);
     spParams.creator.setSeqCount(sequenceCount);
-    auto res = initPacket(memoryId, startAddress, length, updateData);
+    auto res = initPacket(memoryId, currentAddr, length, updateData);
     if (res != returnvalue::OK) {
       return res;
     }
@@ -1170,7 +1171,7 @@ class WriteMemory : public TcBase {
   static const uint16_t META_DATA_LENGTH = 8;
   uint8_t n = 1;
 
-  ReturnValue_t initPacket(uint8_t memoryId, uint32_t startAddr, uint16_t updateDataLen,
+  ReturnValue_t initPacket(uint8_t memoryId, uint32_t currentAddr, uint16_t updateDataLen,
                            uint8_t* updateData) {
     uint8_t* data = payloadStart;
     if (updateDataLen % 2 != 0) {
@@ -1188,7 +1189,7 @@ class WriteMemory : public TcBase {
                                 SerializeIF::Endianness::BIG);
     SerializeAdapter::serialize(&n, &data, &serializedSize, spParams.maxSize,
                                 SerializeIF::Endianness::BIG);
-    SerializeAdapter::serialize(&startAddr, &data, &serializedSize, spParams.maxSize,
+    SerializeAdapter::serialize(&currentAddr, &data, &serializedSize, spParams.maxSize,
                                 SerializeIF::Endianness::BIG);
     SerializeAdapter::serialize(&updateDataLen, &data, &serializedSize, spParams.maxSize,
                                 SerializeIF::Endianness::BIG);

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, 30.0));
+      subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 60.0));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 30.0));
+      subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 60.0));
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0));
   poolManager.subscribeForDiagPeriodicPacket(

mission/acs/RwHandler.cpp

@@ -340,7 +340,7 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
   localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 12.0));
+      subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(

mission/acs/str/StarTrackerHandler.cpp

@@ -1662,7 +1662,7 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(interfaceSet.getSid(), false, 10.0));
   poolManager.subscribeForDiagPeriodicPacket(
-      subdp::DiagnosticsHkPeriodicParams(solutionSet.getSid(), false, 12.0));
+      subdp::DiagnosticsHkPeriodicParams(solutionSet.getSid(), false, 30.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(cameraSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(

mission/controller/AcsController.cpp

@@ -232,7 +232,8 @@ void AcsController::performSafe() {
   acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
-      fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
+      fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(),
+      acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
@@ -251,9 +252,10 @@ void AcsController::performSafe() {
       safeCtrlFailureCounter = 0;
       break;
     case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
-      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
-                          fusedRotRateData.rotRateParallel.value,
-                          fusedRotRateData.rotRateOrthogonal.value,
+      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value,
+                          fusedRotRateSourcesData.rotRateTotalSusMgm.value,
+                          fusedRotRateSourcesData.rotRateParallelSusMgm.value,
+                          fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value,
                           susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
@@ -267,8 +269,8 @@ void AcsController::performSafe() {
       break;
     case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
       safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
-                                     fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
-                                     errAng);
+                                     fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir,
+                                     magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
@@ -355,7 +357,7 @@ void AcsController::performPointingCtrl() {
   }
   acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
-      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
+      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(),
       fusedRotRateData.rotRateSource.value, useMekf);
 
   if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
@@ -387,11 +389,11 @@ void AcsController::performPointingCtrl() {
       quatBI[1] = sensorValues.strSet.caliQy.value;
       quatBI[2] = sensorValues.strSet.caliQz.value;
       quatBI[3] = sensorValues.strSet.caliQw.value;
-      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
       break;
     case acs::ControlModeStrategy::PTGCTRL_QUEST:
       std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
-      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
       break;
     default:
       sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
@@ -555,8 +557,8 @@ void AcsController::performPointingCtrl() {
 void AcsController::handleDetumbling() {
   switch (detumbleState) {
     case DetumbleState::NO_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
+      if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
+          VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) >
               acsParameters.detumbleParameter.omegaDetumbleStart) {
         detumbleCounter++;
       } else if (detumbleCounter > 0) {
@@ -599,8 +601,8 @@ void AcsController::handleDetumbling() {
       detumbleState = DetumbleState::NO_DETUMBLE;
       break;
     case DetumbleState::IN_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
+      if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
+          VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) <
               acsParameters.detumbleParameter.omegaDetumbleEnd) {
         detumbleCounter++;
       } else if (detumbleCounter > 0) {
@@ -747,7 +749,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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 60.0});
   // MGM Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
@@ -757,7 +759,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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 60.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);
@@ -771,7 +773,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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 60.0});
   // SUS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
@@ -788,20 +790,20 @@ 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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 60.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, 10.0});
+  poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 60.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, 10.0});
+  poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 60.0});
   // GPS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
@@ -809,38 +811,37 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
   localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
-  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
+  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 60.0});
   // Attitude Estimation
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
-  poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
+  poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 30.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
   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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 30.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, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.0});
   // Fused Rot Rate
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotSusMgm);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
-  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0});
   // Fused Rot Rate Sources
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
-  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
+  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 60.0});
   return returnvalue::OK;
 }
 
@@ -926,6 +927,17 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
   if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
     detumbleState = DetumbleState::NO_DETUMBLE;
   }
+  if (mode > acs::AcsMode::PTG_IDLE) {
+    poolManager.changeCollectionInterval(ctrlValData.getSid(), 10);
+    poolManager.changeCollectionInterval(actuatorCmdData.getSid(), 10);
+    poolManager.changeCollectionInterval(fusedRotRateData.getSid(), 10);
+    poolManager.changeCollectionInterval(attitudeEstimationData.getSid(), 10);
+  } else {
+    poolManager.changeCollectionInterval(ctrlValData.getSid(), 30);
+    poolManager.changeCollectionInterval(actuatorCmdData.getSid(), 30);
+    poolManager.changeCollectionInterval(fusedRotRateData.getSid(), 30);
+    poolManager.changeCollectionInterval(attitudeEstimationData.getSid(), 30);
+  }
   return ExtendedControllerBase::modeChanged(mode, submode);
 }
 

mission/controller/AcsController.h

@@ -271,9 +271,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   // Fused Rot Rate
   acsctrl::FusedRotRateData fusedRotRateData;
-  PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
-  PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
-  PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotSusMgm = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotSource = PoolEntry<double>(3);
   PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
 
   // Fused Rot Rate Sources

mission/controller/acs/AcsParameters.cpp

@@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x5:
           parameterWrapper->set(onBoardParams.questFilterWeight);
           break;
+        case 0x6:
+          parameterWrapper->set(onBoardParams.questAngleLimit);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -26,6 +26,7 @@ class AcsParameters : public HasParametersIF {
     uint8_t fusedRateFromStr = true;
     uint8_t fusedRateFromQuest = true;
     double questFilterWeight = 0.9;
+    double questAngleLimit = 5 * DEG2RAD;
   } onBoardParams;
 
   struct InertiaEIVE {
@@ -941,7 +942,7 @@ class AcsParameters : public HasParametersIF {
   } sunModelParameters;
 
   struct KalmanFilterParameters {
-    double sensorNoiseStr = 0.1 * DEG2RAD;
+    double sensorNoiseStr = 0.0028 * DEG2RAD;
     double sensorNoiseSus = 8. * DEG2RAD;
     double sensorNoiseMgm = 4. * DEG2RAD;
     double sensorNoiseGyr = 0.1 * DEG2RAD;
@@ -949,7 +950,7 @@ class AcsParameters : public HasParametersIF {
     double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD;  // Angular Random Walk
     double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD;               // Bias Stability
 
-    uint8_t allowStr = false;
+    uint8_t allowStr = true;
   } kalmanFilterParameters;
 
   struct MagnetorquerParameter {

mission/controller/acs/AttitudeEstimation.cpp

@@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
   VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
   VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
 
+  if ((std::acos(VectorOperations<double>::dot(normSusB, normMgmB)) <
+       acsParameters->onBoardParams.questAngleLimit) or
+      (std::acos(VectorOperations<double>::dot(normSusI, normMgmI)) <
+       acsParameters->onBoardParams.questAngleLimit)) {
+    {
+      PoolReadGuard pg{attitudeEstimationData};
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
+        attitudeEstimationData->quatQuest.setValid(false);
+      }
+    }
+    return;
+  }
+
   // Create Helper Vectors
   double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
          helperSum[3] = {0, 0, 0};

mission/controller/acs/FusedRotationEstimation.cpp

@@ -19,13 +19,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
                                            acsParameters->onBoardParams.fusedRateFromStr)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateTotal.value,
+      std::memcpy(fusedRotRateData->rotRateTotalSource.value,
                   fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateTotal.setValid(true);
+      fusedRotRateData->rotRateTotalSource.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
       fusedRotRateData->rotRateSource.setValid(true);
     }
@@ -34,41 +30,38 @@ void FusedRotationEstimation::estimateFusedRotationRate(
               acsParameters->onBoardParams.fusedRateFromQuest)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateTotal.value,
+      std::memcpy(fusedRotRateData->rotRateTotalSource.value,
                   fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateTotal.setValid(true);
+      fusedRotRateData->rotRateTotalSource.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
       fusedRotRateData->rotRateSource.setValid(true);
     }
   } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
-                fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateOrthogonal.setValid(
-        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
-    std::memcpy(fusedRotRateData->rotRateParallel.value,
-                fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateParallel.setValid(
-        fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
-    std::memcpy(fusedRotRateData->rotRateTotal.value,
+    std::memcpy(fusedRotRateData->rotRateTotalSource.value,
                 fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateTotal.setValid(true);
+    fusedRotRateData->rotRateTotalSource.setValid(true);
     fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
     fusedRotRateData->rotRateSource.setValid(true);
   } else {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->setValidity(false, true);
+      std::memcpy(fusedRotRateData->rotRateTotalSource.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotalSource.setValid(false);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
       fusedRotRateData->rotRateSource.setValid(true);
     }
   }
+  if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
+    std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value,
+                fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
+    fusedRotRateData->rotRateTotalSusMgm.setValid(true);
+  } else {
+    PoolReadGuard pg(fusedRotRateData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotalSusMgm.setValid(false);
+    }
+  }
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateStr(

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -114,12 +114,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
     return result;
   }
 
-  double measSensMatrix[matrixDimensionFactor][6] = {{0}},
-         measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
-         measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0};
+  double measSensMatrix[matrixDimensionFactor][6] = {},
+         measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {},
+         measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {};
   kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
 
-  double kalmanGain[6][matrixDimensionFactor] = {{0}};
+  double kalmanGain[6][matrixDimensionFactor];
+  std::memset(kalmanGain, 0, sizeof(kalmanGain));
   result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
   if (result != returnvalue::OK) {
     reset(attitudeEstimationData);
@@ -342,10 +343,11 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain(
     double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
     acsctrl::AttitudeEstimationData *attitudeEstimationData) {
   // Kalman Gain: K = P * H' / (H * P * H' + R)
-  double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
-         invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
-         residualCov[6][matrixDimensionFactor] = {{0}},
-         measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}};
+  double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {},
+         invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {};
+  double residualCov[6][matrixDimensionFactor], measSensMatrixTransposed[6][matrixDimensionFactor];
+  std::memset(residualCov, 0, sizeof(residualCov));
+  std::memset(measSensMatrixTransposed, 0, sizeof(measSensMatrixTransposed));
 
   MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
                                       matrixDimensionFactor, 6);
@@ -382,8 +384,7 @@ void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *me
 
 void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec,
                                                     double *estVec) {
-  double stateVecErr[6] = {0, 0, 0, 0, 0, 0};
-  double plantOutputDiff[matrixDimensionFactor] = {0};
+  double stateVecErr[6] = {0, 0, 0, 0, 0, 0}, plantOutputDiff[matrixDimensionFactor] = {};
   VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor);
   MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6,
                                      matrixDimensionFactor, 1);

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -129,9 +129,8 @@ enum PoolIds : lp_id_t {
   RW_TARGET_SPEED,
   MTQ_TARGET_DIPOLE,
   // Fused Rotation Rate
-  ROT_RATE_ORTHOGONAL,
-  ROT_RATE_PARALLEL,
-  ROT_RATE_TOTAL,
+  ROT_RATE_TOT_SUSMGM,
+  ROT_RATE_TOT_SOURCE,
   ROT_RATE_SOURCE,
   // Fused Rotation Rate Sources
   ROT_RATE_ORTHOGONAL_SUSMGM,
@@ -151,7 +150,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
 static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
-static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
+static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
 static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
 
 /**
@@ -318,10 +317,10 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
   FusedRotRateData(HasLocalDataPoolIF* hkOwner)
       : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
 
-  lp_vec_t<double, 3> rotRateOrthogonal =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
-  lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
-  lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
+  lp_vec_t<double, 3> rotRateTotalSusMgm =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOT_SUSMGM, this);
+  lp_vec_t<double, 3> rotRateTotalSource =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOT_SOURCE, this);
   lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
 
  private:

mission/system/acs/acsModeTree.cpp

@@ -105,7 +105,7 @@ Subsystem& satsystem::acs::init() {
   };
   // Build TARGET PT transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
@@ -114,7 +114,7 @@ Subsystem& satsystem::acs::init() {
         ctxc);
 
   // Build SUS board transition
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, SUS_BOARD_NML_TRANS.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, SUS_BOARD_NML_TRANS.second, true);
   check(ACS_SUBSYSTEM.addTable(TableEntry(SUS_BOARD_NML_TRANS.first, &SUS_BOARD_NML_TRANS.second)),
         ctxc);
 
@@ -200,14 +200,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT,
       ACS_TABLE_SAFE_TGT.second, true);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TGT.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TGT.second, true);
   check(ss.addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc);
 
   // Build SAFE transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
   iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
   iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
   check(ss.addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true),
@@ -257,14 +257,14 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TGT.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TGT.second, true);
   ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);
 
   // Build IDLE transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
@@ -307,7 +307,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
@@ -356,7 +356,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
@@ -409,7 +409,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
@@ -462,7 +462,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0,
       ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
+  iht(objects::SUS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
       true);
   iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
       true);