Merge pull request 'Prep v7.1.1' (#874) from prep-v7.1.1 into main

Reviewed-on: #874

CHANGELOG.md

@@ -16,6 +16,63 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v7.7.1] 2024-03-06
+
+- Bumped `eive-tmtc` to v6.1.1
+- Bumped `eive-fsfw`
+
+## Added
+
+- The `CoreController` now sets the leap seconds on initalization. They are stored in a persistent
+  file. If the file does yet not exist, it will be created. The leap seconds can be updated using an
+  action command. This will also update the file.
+
+## Fixed
+
+- Fixed wrong dimension of a matrix within the `MEKF`, which would lead to a seg fault, if the
+  star tracker was available.
+- Fixed case in which control values within the `AcsController` could become NaN.
+
+# [v7.7.0] 2024-02-29
+
+- Bumped `eive-tmtc` to v6.1.0
+- Bumped `eive-fsfw`
+
+## Fixed
+
+- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report
+  and latchup status report.
+- PLOC SUPV latchup report could not be handled previously.
+- Bugfix in PLOC SUPV latchup report parsing.
+- Bugfix in PLOC MPSoC HK set: Set and variables were not set valid.
+- The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
+  and is triggered by the `AcsController` now.
+- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
+- Fixed calculation of desaturation torque for faulty RWs.
+- Fixed bugs within the `MEKF` and simplified the code.
+
+## Changed
+
+- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes
+- QUEST and STR rates are now allowed per default
+- Changed PTG Strat priorities to favor STR before MEKF.
+- Increased message queue depth and maximum number of handled messages per cycle for
+  `PusServiceBase` based classes (especially PUS scheduler).
+- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw`
+- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This
+  also limits the rotation for the reference target quaternion to prevent spikes in required
+  rotation rates.
+- Updated QUEST and Sun Vector Params to new values.
+- Removed the satellites's angular momentum from desaturation calculation.
+- Bumped internal `sagittactl` library to v11.11.
+
+## Added
+
+- Updated STR handler to unlock and allow using the secondary firmware slot.
+- STR handling for new BlobStats TM set.
+- Added new action command to update the standard deviations within the `MEKF` from the
+  `AcsParameters`.
+
 # [v7.6.1] 2024-02-05
 
 ## Changed

CMakeLists.txt

@@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 7)
-set(OBSW_VERSION_MINOR 6)
+set(OBSW_VERSION_MINOR 7)
 set(OBSW_VERSION_REVISION 1)
 
 # set(CMAKE_VERBOSE TRUE)

bsp_hosted/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 319 translations.
+ * @brief    Auto-generated event translation file. Contains 320 translations.
  * @details
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateEvents.h"
 
@@ -104,6 +104,7 @@ const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
 const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
 const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
+const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -525,6 +526,8 @@ const char *translateEvents(Event event) {
       return TLE_FILE_READ_FAILED_STRING;
     case (11210):
       return PTG_RATE_VIOLATION_STRING;
+    case (11211):
+      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

bsp_hosted/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 175 translations.
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateObjects.h"
 

bsp_q7s/core/CoreController.cpp

@@ -480,6 +480,16 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
       successRecipient = commandedBy;
       return returnvalue::OK;
     }
+    case (UPDATE_LEAP_SECONDS): {
+      if (size != sizeof(uint16_t)) {
+        return HasActionsIF::INVALID_PARAMETERS;
+      }
+      ReturnValue_t result = actionUpdateLeapSeconds(data);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -1411,6 +1421,9 @@ void CoreController::performMountedSdCardOperations() {
       if (not timeFileInitDone) {
         initClockFromTimeFile();
       }
+      if (not leapSecondsInitDone) {
+        initLeapSeconds();
+      }
       performRebootWatchdogHandling(false);
       performRebootCountersHandling(false);
     }
@@ -2066,6 +2079,71 @@ ReturnValue_t CoreController::backupTimeFileHandler() {
   return returnvalue::OK;
 }
 
+void CoreController::initLeapSeconds() {
+  ReturnValue_t result = initLeapSecondsFromFile();
+  if (result != returnvalue::OK) {
+    Clock::setLeapSeconds(config::LEAP_SECONDS);
+    writeLeapSecondsToFile(config::LEAP_SECONDS);
+  }
+  leapSecondsInitDone = true;
+}
+
+ReturnValue_t CoreController::initLeapSecondsFromFile() {
+  std::string fileName = currMntPrefix + LEAP_SECONDS_FILE;
+  std::error_code e;
+  if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e)) {
+    std::ifstream leapSecondsFile(fileName);
+    std::string nextWord;
+    std::getline(leapSecondsFile, nextWord);
+    std::istringstream iss(nextWord);
+    iss >> nextWord;
+    if (iss.bad() or nextWord != "LEAP") {
+      return returnvalue::FAILED;
+    }
+    iss >> nextWord;
+    if (iss.bad() or nextWord != "SECONDS:") {
+      return returnvalue::FAILED;
+    }
+    iss >> nextWord;
+    uint16_t leapSeconds = 0;
+    leapSeconds = std::stoi(nextWord.c_str());
+    if (iss.bad()) {
+      return returnvalue::FAILED;
+    }
+    Clock::setLeapSeconds(leapSeconds);
+    return returnvalue::OK;
+  }
+  sif::error
+      << "CoreController::leapSecondsFileHandler: Initalization of leap seconds from file failed"
+      << std::endl;
+  return returnvalue::FAILED;
+};
+
+ReturnValue_t CoreController::writeLeapSecondsToFile(const uint16_t leapSeconds) {
+  std::string fileName = currMntPrefix + LEAP_SECONDS_FILE;
+  if (not sdcMan->isSdCardUsable(std::nullopt)) {
+    return returnvalue::FAILED;
+  }
+  std::ofstream leapSecondsFile(fileName.c_str(), std::ofstream::out | std::ofstream::trunc);
+  if (not leapSecondsFile.good()) {
+    sif::error << "CoreController::leapSecondsFileHandler: Error opening leap seconds file: "
+               << strerror(errno) << std::endl;
+    return returnvalue::FAILED;
+  }
+  leapSecondsFile << "LEAP SECONDS: " << leapSeconds << std::endl;
+  return returnvalue::OK;
+};
+
+ReturnValue_t CoreController::actionUpdateLeapSeconds(const uint8_t *data) {
+  uint16_t leapSeconds = data[1] | (data[0] << 8);
+  ReturnValue_t result = writeLeapSecondsToFile(leapSeconds);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  Clock::setLeapSeconds(leapSeconds);
+  return returnvalue::OK;
+}
+
 ReturnValue_t CoreController::initClockFromTimeFile() {
   using namespace GpsHyperion;
   using namespace std;

bsp_q7s/core/CoreController.h

@@ -150,6 +150,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
       std::string(core::LEGACY_REBOOT_WATCHDOG_FILE_NAME);
   const std::string REBOOT_WATCHDOG_FILE =
       "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_WATCHDOG_FILE_NAME);
+  const std::string LEAP_SECONDS_FILE =
+      "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::LEAP_SECONDS_FILE_NAME);
   const std::string BACKUP_TIME_FILE =
       "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::TIME_FILE_NAME);
   const std::string REBOOT_COUNTERS_FILE =
@@ -296,6 +298,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
 
   std::string currMntPrefix;
   bool timeFileInitDone = false;
+  bool leapSecondsInitDone = false;
   bool performOneShotSdCardOpsSwitch = false;
   uint8_t shortSdCardCdCounter = 0;
 #if OBSW_THREAD_TRACING == 1
@@ -335,7 +338,11 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
   void performMountedSdCardOperations();
   ReturnValue_t initVersionFile();
 
+  void initLeapSeconds();
+  ReturnValue_t initLeapSecondsFromFile();
   ReturnValue_t initClockFromTimeFile();
+  ReturnValue_t actionUpdateLeapSeconds(const uint8_t* data);
+  ReturnValue_t writeLeapSecondsToFile(const uint16_t leapSeconds);
   ReturnValue_t performSdCardCheck();
   ReturnValue_t backupTimeFileHandler();
   ReturnValue_t initBootCopyFile();

bsp_q7s/objectFactory.cpp

@@ -951,7 +951,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManage
   auto cfgGetter = new StrConfigPathGetter(sdcMan);
   auto starTracker =
       new StarTrackerHandler(objects::STAR_TRACKER, objects::STR_COM_IF, starTrackerCookie,
-                             strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter);
+                             strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter, sdcMan);
   starTracker->setPowerSwitcher(pwrSwitcher);
   starTracker->connectModeTreeParent(*strAssy);
   starTracker->setCustomFdir(strFdir);

common/config/eive/definitions.h

@@ -20,6 +20,9 @@ static constexpr char OBSW_VERSION_FILE_PATH[] = "/usr/share/eive-obsw/obsw_vers
 // ISO8601 timestamp.
 static constexpr char FILE_DATE_FORMAT[] = "%FT%H%M%SZ";
 
+// Leap Seconds as of 2024-03-04
+static constexpr uint16_t LEAP_SECONDS = 37;
+
 static constexpr uint16_t EIVE_PUS_APID = 0x65;
 static constexpr uint16_t EIVE_CFDP_APID = 0x66;
 static constexpr uint16_t EIVE_LOCAL_CFDP_ENTITY_ID = EIVE_CFDP_APID;

generators/bsp_hosted_events.csv

@@ -98,6 +98,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
 11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
 11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
+11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h

generators/bsp_q7s_events.csv

@@ -98,6 +98,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
 11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
 11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
+11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h

generators/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 319 translations.
+ * @brief    Auto-generated event translation file. Contains 320 translations.
  * @details
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateEvents.h"
 
@@ -104,6 +104,7 @@ const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
 const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
 const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
+const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -525,6 +526,8 @@ const char *translateEvents(Event event) {
       return TLE_FILE_READ_FAILED_STRING;
     case (11210):
       return PTG_RATE_VIOLATION_STRING;
+    case (11211):
+      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

generators/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateObjects.h"
 

linux/acs/StrComHandler.cpp

@@ -175,7 +175,8 @@ void StrComHandler::setDownloadImageName(std::string filename) {
 
 void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; }
 
-ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
+ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
+                                                 startracker::FirmwareTarget target) {
   {
     MutexGuard mg(lock);
     if (state != InternalState::SLEEPING) {
@@ -192,8 +193,13 @@ ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
   if (not std::filesystem::exists(flashWrite.fullname)) {
     return FILE_NOT_EXISTS;
   }
-  flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST);
-  flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST);
+  if (target == startracker::FirmwareTarget::MAIN) {
+    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_MAIN);
+    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_MAIN);
+  } else if (target == startracker::FirmwareTarget::BACKUP) {
+    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_BACKUP);
+    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_BACKUP);
+  }
   {
     MutexGuard mg(lock);
     replyWasReceived = false;
@@ -264,7 +270,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
       file.close();
       return returnvalue::OK;
     }
-    arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
+    prv_arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, downloadReq.position);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
@@ -275,7 +281,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
       file.close();
       return result;
     }
-    result = checkActionReply(replySize);
+    result = checkActionReply(replySize, "downloading image");
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
         serial::flushRxBuf(serialPort);
@@ -343,12 +349,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
     }
     file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
     file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART);
-    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
+    prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, uploadReq.position);
     if (result != returnvalue::OK) {
       return returnvalue::FAILED;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "sky image upload");
     if (result != returnvalue::OK) {
       return result;
     }
@@ -369,12 +375,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
     file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg);
     file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
     file.close();
-    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
+    prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, uploadReq.position);
     if (result != returnvalue::OK) {
       return returnvalue::FAILED;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "sky image upload");
     if (result != returnvalue::OK) {
       return result;
     }
@@ -388,8 +394,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
 ReturnValue_t StrComHandler::performFirmwareUpdate() {
   using namespace startracker;
   ReturnValue_t result = returnvalue::OK;
-  result = unlockAndEraseRegions(static_cast<uint32_t>(startracker::FirmwareRegions::FIRST),
-                                 static_cast<uint32_t>(startracker::FirmwareRegions::LAST));
+  result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -440,12 +445,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
       bytesWrittenInRegion = 0;
     }
     req.address = bytesWrittenInRegion;
-    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
+    prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "firmware image upload");
     if (result != returnvalue::OK) {
       return result;
     }
@@ -483,12 +488,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
     req.length = remainingBytes;
     totalBytesWritten += CHUNK_SIZE;
     bytesWrittenInRegion += remainingBytes;
-    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
+    prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "flash write");
     if (result != returnvalue::OK) {
       return result;
     }
@@ -531,7 +536,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
     } else {
       req.length = CHUNK_SIZE;
     }
-    arc_pack_read_action_req(&req, cmdBuf.data(), &size);
+    prv_arc_pack_read_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
@@ -542,7 +547,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
       file.close();
       return result;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "flash read");
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
         serial::flushRxBuf(serialPort);
@@ -584,7 +589,7 @@ ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) {
   return readOneReply(failParameter);
 }
 
-ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
+ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) {
   uint8_t type = startracker::getReplyFrameType(replyPtr);
   if (type != TMTC_ACTIONREPLY) {
     sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl;
@@ -592,7 +597,7 @@ ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
   }
   uint8_t status = startracker::getStatusField(replyPtr);
   if (status != ArcsecDatalinkLayer::STATUS_OK) {
-    sif::warning << "StrHelper::checkActionReply: Status failure: "
+    sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": "
                  << static_cast<unsigned int>(status) << std::endl;
     return STATUS_ERROR;
   }
@@ -744,23 +749,26 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
   struct UnlockActionRequest unlockReq;
   struct EraseActionRequest eraseReq;
   uint32_t size = 0;
-  for (uint32_t idx = from; idx <= to; idx++) {
+  for (uint32_t idx = from; idx < to; idx++) {
     unlockReq.region = idx;
-    unlockReq.code = startracker::region_secrets::secret[idx];
-    arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
+    unlockReq.code = startracker::region_secrets::SECRETS[idx];
+    prv_arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
     result = sendAndRead(size, unlockReq.region);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen);
+    result = checkActionReply(replyLen, "unlocking region");
     if (result != returnvalue::OK) {
       sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id "
                    << static_cast<unsigned int>(unlockReq.region) << std::endl;
       return result;
     }
     eraseReq.region = idx;
-    arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
+    prv_arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
     result = sendAndRead(size, eraseReq.region);
+    if (result != returnvalue::OK) {
+    }
+    result = checkActionReply(replyLen, "erasing region");
     if (result != returnvalue::OK) {
       sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id "
                    << static_cast<unsigned int>(eraseReq.region) << std::endl;

linux/acs/StrComHandler.h

@@ -6,6 +6,7 @@
 #include <string>
 
 #include "OBSWConfig.h"
+#include "mission/acs/str/strHelpers.h"
 
 #ifdef XIPHOS_Q7S
 #include "bsp_q7s/fs/SdCardManager.h"
@@ -127,7 +128,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
    * @param fullname    Full name including absolute path of file containing firmware
    *                    update.
    */
-  ReturnValue_t startFirmwareUpdate(std::string fullname);
+  ReturnValue_t startFirmwareUpdate(std::string fullname, startracker::FirmwareTarget target);
 
   /**
    * @brief   Starts the flash read procedure
@@ -334,7 +335,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
    *
    * @return  returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED
    */
-  ReturnValue_t checkActionReply(size_t replySize);
+  ReturnValue_t checkActionReply(size_t replySize, const char *context);
 
   /**
    * @brief   Checks the position field in a star tracker upload/download reply.

linux/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 319 translations.
+ * @brief    Auto-generated event translation file. Contains 320 translations.
  * @details
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateEvents.h"
 
@@ -104,6 +104,7 @@ const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
 const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
 const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
+const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -525,6 +526,8 @@ const char *translateEvents(Event event) {
       return TLE_FILE_READ_FAILED_STRING;
     case (11210):
       return PTG_RATE_VIOLATION_STRING;
+    case (11211):
+      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

linux/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2024-01-30 09:10:05
+ * Generated on: 2024-02-29 13:15:00
  */
 #include "translateObjects.h"
 

linux/payload/FreshSupvHandler.cpp

@@ -900,6 +900,14 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
         }
         break;
       }
+      case (Apid::LATCHUP_MON): {
+        if (tmReader.getServiceId() ==
+            static_cast<uint8_t>(supv::tm::LatchupMonId::LATCHUP_STATUS_REPORT)) {
+          handleLatchupStatusReport(receivedData);
+          continue;
+        }
+        break;
+      }
       case (Apid::ADC_MON): {
         if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::AdcMonId::ADC_REPORT)) {
           genericHandleTm("ADC", receivedData, adcReport, supv::Apid::ADC_MON,
@@ -1391,15 +1399,17 @@ ReturnValue_t FreshSupvHandler::verifyPacket(const uint8_t* start, size_t foundL
 
 ReturnValue_t FreshSupvHandler::handleBootStatusReport(const uint8_t* data) {
   ReturnValue_t result = returnvalue::OK;
-
   result = verifyPacket(data, tmReader.getFullPacketLen());
-
   if (result == result::CRC_FAILURE) {
     sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid"
                   " crc"
                << std::endl;
     return result;
   }
+  PoolReadGuard pg(&bootStatusReport);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return pg.getReadResult();
+  }
 
   const uint8_t* payloadStart = tmReader.getPayloadStart();
   uint16_t offset = 0;
@@ -1463,13 +1473,17 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) {
   ReturnValue_t result = returnvalue::OK;
 
   result = verifyPacket(data, tmReader.getFullPacketLen());
-
   if (result == result::CRC_FAILURE) {
     sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has "
                << "invalid crc" << std::endl;
     return result;
   }
 
+  PoolReadGuard pg(&latchupStatusReport);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return pg.getReadResult();
+  }
+
   const uint8_t* payloadData = tmReader.getPayloadStart();
   uint16_t offset = 0;
   latchupStatusReport.id = *(payloadData + offset);
@@ -1486,10 +1500,10 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) {
   offset += 2;
   latchupStatusReport.cnt5 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
   offset += 2;
-  latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(data + offset + 1);
+  latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
   offset += 2;
   uint16_t msec = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
-  latchupStatusReport.isSet = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS;
+  latchupStatusReport.isSynced = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS;
   latchupStatusReport.timeMsec = msec & (~(1 << latchupStatusReport.IS_SET_BIT_POS));
   offset += 2;
   latchupStatusReport.timeSec = *(payloadData + offset);

linux/payload/PlocMpsocHandler.cpp

@@ -998,6 +998,7 @@ ReturnValue_t PlocMpsocHandler::handleGetHkReport(const uint8_t* data) {
   if (result != returnvalue::OK) {
     return result;
   }
+  hkReport.setValidity(true, true);
   return returnvalue::OK;
 }
 

linux/payload/plocSupvDefs.h

@@ -1805,7 +1805,7 @@ class LatchupStatusReport : public StaticLocalDataSet<LATCHUP_RPT_SET_ENTRIES> {
   lp_var_t<uint8_t> timeMon = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_MON, this);
   lp_var_t<uint8_t> timeYear =
       lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_YEAR, this);
-  lp_var_t<uint8_t> isSet = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
+  lp_var_t<uint8_t> isSynced = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
 
   static const uint8_t IS_SET_BIT_POS = 15;
 };

mission/acs/str/ArcsecDatalinkLayer.cpp

@@ -32,7 +32,7 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF
     size_t encodedDataSize = 0;
     slip_error_t slipError =
         slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
-                          idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID);
+                          idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0);
     decodeRingBuf.deleteData(idx + 1);
     switch (slipError) {
       case (SLIP_OK): {
@@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); }
 
 void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
                                       size_t& size) {
-  slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID);
+  slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID);
   if (txFrame != nullptr) {
     *txFrame = txEncoded;
   }

mission/acs/str/StarTrackerHandler.cpp

@@ -5,7 +5,12 @@
 #include <mission/acs/str/strHelpers.h>
 #include <mission/acs/str/strJsonCommands.h>
 
+#include <string>
+
+#include "fsfw/filesystem/HasFileSystemIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/returnvalues/returnvalue.h"
+#include "mission/memory/SdCardMountedIF.h"
 
 extern "C" {
 #include <sagitta/client/actionreq.h>
@@ -16,7 +21,6 @@ extern "C" {
 }
 
 #include <atomic>
-#include <fstream>
 #include <thread>
 
 #include "OBSWConfig.h"
@@ -27,7 +31,8 @@ std::atomic_bool JCFG_DONE(false);
 
 StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                                        StrComHandler* strHelper, power::Switch_t powerSwitch,
-                                       startracker::SdCardConfigPathGetter& cfgPathGetter)
+                                       startracker::SdCardConfigPathGetter& cfgPathGetter,
+                                       SdCardMountedIF& sdCardIF)
     : DeviceHandlerBase(objectId, comIF, comCookie),
       temperatureSet(this),
       versionSet(this),
@@ -58,8 +63,10 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       centroidSet(this),
       centroidsSet(this),
       contrastSet(this),
+      blobStatsSet(this),
       strHelper(strHelper),
       powerSwitch(powerSwitch),
+      sdCardIF(sdCardIF),
       cfgPathGetter(cfgPathGetter) {
   if (comCookie == nullptr) {
     sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
@@ -138,6 +145,9 @@ ReturnValue_t StarTrackerHandler::initialize() {
   // delay whole satellite boot process.
   reloadJsonCfgFile();
 
+  // Default firmware target is always initialized from persistent file.
+  loadTargetFirmwareFromPersistentCfg();
+
   EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
   if (manager == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -165,6 +175,19 @@ ReturnValue_t StarTrackerHandler::initialize() {
   return returnvalue::OK;
 }
 
+void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() {
+  const char* prefix = sdCardIF.getCurrentMountPrefix();
+  std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
+  std::ifstream ifile(path);
+  if (ifile.is_open() and !ifile.bad()) {
+    std::string targetStr;
+    std::getline(ifile, targetStr);
+    if (targetStr == "backup") {
+      firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP);
+    }
+  }
+}
+
 bool StarTrackerHandler::reloadJsonCfgFile() {
   jcfgCountdown.resetTimer();
   auto strCfgPath = cfgPathGetter.getCfgPath();
@@ -307,21 +330,11 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
       strHelper->setFlashReadFilename(std::string(reinterpret_cast<const char*>(data), size));
       return EXECUTION_FINISHED;
     }
-    case (startracker::FIRMWARE_UPDATE): {
-      result = DeviceHandlerBase::acceptExternalDeviceCommands();
-      if (result != returnvalue::OK) {
-        return result;
-      }
-      if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
-        return FILE_PATH_TOO_LONG;
-      }
-      result =
-          strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size));
-      if (result != returnvalue::OK) {
-        return result;
-      }
-      strHelperHandlingSpecialRequest = true;
-      return EXECUTION_FINISHED;
+    case (startracker::FIRMWARE_UPDATE_MAIN): {
+      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN);
+    }
+    case (startracker::FIRMWARE_UPDATE_BACKUP): {
+      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP);
     }
     default:
       break;
@@ -330,6 +343,23 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
   reinitNextSetParam = true;
   return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
 }
+ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
+                                                              startracker::FirmwareTarget target) {
+  ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
+    return FILE_PATH_TOO_LONG;
+  }
+  result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size),
+                                          target);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  strHelperHandlingSpecialRequest = true;
+  return EXECUTION_FINISHED;
+}
 
 void StarTrackerHandler::performOperationHook() {
   EventMessage event;
@@ -543,7 +573,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       Clock::getClock(&tv);
       setTimeRequest.unixTime =
           (static_cast<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(tv.tv_usec));
-      arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
+      prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
       size_t serLen = 0;
       // Time in milliseconds. Manual serialization because arcsec API ignores endianness.
       SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen,
@@ -568,8 +598,12 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       prepareRequestContrastTm();
       return returnvalue::OK;
     }
+    case (startracker::REQ_BLOB_STATS): {
+      prepareRequestBlobStatsTm();
+      return returnvalue::OK;
+    }
     case (startracker::BOOT): {
-      prepareBootCommand();
+      prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
       return returnvalue::OK;
     }
     case (startracker::REQ_VERSION): {
@@ -856,6 +890,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -1053,6 +1089,12 @@ void StarTrackerHandler::resetSecondaryTmSet() {
       histogramSet.setValidity(false, true);
     }
   }
+  {
+    PoolReadGuard pg(&blobStatsSet);
+    if (pg.getReadResult() == returnvalue::OK) {
+      histogramSet.setValidity(false, true);
+    }
+  }
 }
 
 void StarTrackerHandler::bootBootloader() {
@@ -1201,6 +1243,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleTm(packet, histogramSet, "REQ_HISTO");
       break;
     }
+    case (startracker::REQ_BLOB_STATS): {
+      result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS");
+      break;
+    }
     case (startracker::SUBSCRIPTION):
     case (startracker::LOGLEVEL):
     case (startracker::LOGSUBSCRIPTION):
@@ -1602,6 +1648,13 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
   localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
 
+  localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry<uint64_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry<uint8_t>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry<uint8_t>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry<uint8_t>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry<uint8_t>(16));
+
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
@@ -1630,6 +1683,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
       subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
@@ -1659,7 +1714,8 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
     case startracker::UPLOAD_IMAGE:
     case startracker::DOWNLOAD_IMAGE:
     case startracker::FLASH_READ:
-    case startracker::FIRMWARE_UPDATE: {
+    case startracker::FIRMWARE_UPDATE_BACKUP:
+    case startracker::FIRMWARE_UPDATE_MAIN: {
       return DeviceHandlerBase::acceptExternalDeviceCommands();
       default:
         break;
@@ -1875,6 +1931,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
       *foundId = startracker::REQ_BLOB;
       break;
     }
+    case (startracker::ID::BLOB_STATS): {
+      *foundId = startracker::REQ_BLOB_STATS;
+      break;
+    }
     case (startracker::ID::BLOBS): {
       *foundId = startracker::REQ_BLOBS;
       break;
@@ -1956,10 +2016,10 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command
   return result;
 }
 
-void StarTrackerHandler::prepareBootCommand() {
+void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
   uint32_t length = 0;
-  struct BootActionRequest bootRequest = {BOOT_REGION_ID};
-  arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
+  struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)};
+  prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -1992,7 +2052,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD
     return result;
   }
   uint32_t rawCmdLength = 0;
-  arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
+  prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
   rawPacket = commandBuffer;
   rawPacketLen = rawCmdLength;
   checksumCmd.rememberRegion = req.region;
@@ -2011,7 +2071,7 @@ void StarTrackerHandler::prepareTimeRequest() {
 void StarTrackerHandler::preparePingRequest() {
   uint32_t length = 0;
   struct PingActionRequest pingRequest = {PING_ID};
-  arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
+  prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2040,7 +2100,7 @@ void StarTrackerHandler::preparePowerRequest() {
 void StarTrackerHandler::prepareSwitchToBootloaderCmd() {
   uint32_t length = 0;
   struct RebootActionRequest rebootReq {};
-  arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
+  prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2049,7 +2109,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
   uint32_t length = 0;
   struct CameraActionRequest camReq;
   camReq.actionid = *commandData;
-  arc_pack_camera_action_req(&camReq, commandBuffer, &length);
+  prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2115,6 +2175,14 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
   return returnvalue::OK;
 }
 
+ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() {
+  uint32_t length = 0;
+  arc_tm_pack_blobstats_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
   uint32_t length = 0;
   arc_tm_pack_centroids_req(commandBuffer, &length);
@@ -2389,7 +2457,8 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
         internalState = InternalState::DONE;
       }
       break;
-    case startracker::Program::FIRMWARE:
+    case startracker::Program::FIRMWARE_BACKUP:
+    case startracker::Program::FIRMWARE_MAIN: {
       if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
         startupState = StartupState::BOOT_BOOTLOADER;
       }
@@ -2400,9 +2469,10 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
         internalState = InternalState::FAILED_BOOTLOADER_BOOT;
       }
       break;
+    }
     default:
-      sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
-                   << std::endl;
+      sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID "
+                   << static_cast<int>(versionSet.program.value) << std::endl;
       return INVALID_PROGRAM;
   }
   return returnvalue::OK;
@@ -2862,17 +2932,19 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_MATCHED_CENTROIDS:
     case startracker::REQ_BLOB:
     case startracker::REQ_BLOBS:
+    case startracker::REQ_BLOB_STATS:
     case startracker::REQ_CENTROID:
     case startracker::REQ_CENTROIDS:
     case startracker::REQ_CONTRAST: {
-      if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
+      if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }
       break;
     }
-    case startracker::FIRMWARE_UPDATE:
+    case startracker::FIRMWARE_UPDATE_MAIN:
+    case startracker::FIRMWARE_UPDATE_BACKUP:
     case startracker::FLASH_READ:
-      if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) {
+      if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) {
         return STARTRACKER_NOT_RUNNING_BOOTLOADER;
       }
       break;
@@ -2883,3 +2955,42 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
 }
 
 ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; }
+
+ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
+                                               ParameterWrapper* parameterWrapper,
+                                               const ParameterWrapper* newValues,
+                                               uint16_t startAtIndex) {
+  auto firmwareTargetUpdate = [&](bool persistent) {
+    uint8_t value = 0;
+    newValues->getElement(&value);
+    if (value != static_cast<uint8_t>(startracker::FirmwareTarget::MAIN) &&
+        value != static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP)) {
+      return HasParametersIF::INVALID_VALUE;
+    }
+    parameterWrapper->set(firmwareTargetRaw);
+    if (persistent) {
+      if (sdCardIF.isSdCardUsable(std::nullopt)) {
+        const char* prefix = sdCardIF.getCurrentMountPrefix();
+        std::filesystem::path path =
+            std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
+        std::ofstream of(path, std::ofstream::out | std::ofstream::trunc);
+        if (value == static_cast<uint8_t>(startracker::FirmwareTarget::MAIN)) {
+          of << "main\n";
+        } else {
+          of << "backup\n";
+        }
+      } else {
+        return HasFileSystemIF::FILESYSTEM_INACTIVE;
+      }
+    };
+    return returnvalue::OK;
+  };
+  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) {
+    return firmwareTargetUpdate(false);
+  }
+  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) {
+    return firmwareTargetUpdate(true);
+  }
+  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
+                                         startAtIndex);
+}

mission/acs/str/StarTrackerHandler.h

@@ -11,10 +11,7 @@
 #include <set>
 #include <thread>
 
-#include "OBSWConfig.h"
-#include "devices/powerSwitcherList.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
-#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/timemanager/Countdown.h"
 
 extern "C" {
@@ -45,7 +42,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
    */
   StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                      StrComHandler* strHelper, power::Switch_t powerSwitch,
-                     startracker::SdCardConfigPathGetter& cfgPathGetter);
+                     startracker::SdCardConfigPathGetter& cfgPathGetter, SdCardMountedIF& sdCardIF);
   virtual ~StarTrackerHandler();
 
   ReturnValue_t initialize() override;
@@ -61,6 +58,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   Submode_t getInitialSubmode() override;
 
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
+                             const ParameterWrapper* newValues, uint16_t startAtIndex) override;
+
  protected:
   void doStartUp() override;
   void doShutDown() override;
@@ -161,7 +161,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   // Ping request will reply ping with this ID (data field)
   static const uint32_t PING_ID = 0x55;
-  static const uint32_t BOOT_REGION_ID = 1;
+  uint8_t firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::MAIN);
+
   static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
   static const uint32_t MUTEX_TIMEOUT = 20;
   static const uint32_t BOOT_TIMEOUT = 1000;
@@ -215,6 +216,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::CentroidSet centroidSet;
   startracker::CentroidsSet centroidsSet;
   startracker::ContrastSet contrastSet;
+  startracker::BlobStatsSet blobStatsSet;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -314,12 +316,14 @@ class StarTrackerHandler : public DeviceHandlerBase {
   std::set<DeviceCommandId_t> additionalRequestedTm{};
   std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;
 
+  SdCardMountedIF& sdCardIF;
   startracker::SdCardConfigPathGetter& cfgPathGetter;
 
   /**
    * @brief   Handles internal state
    */
   void handleInternalState();
+  void loadTargetFirmwareFromPersistentCfg();
 
   /**
    * @brief   Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
@@ -380,7 +384,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
    * @brief   Fills command buffer with data to boot image (works only when star tracker is
    *          in bootloader mode).
    */
-  void prepareBootCommand();
+  void prepareBootCommand(startracker::FirmwareTarget target);
 
   /**
    * @brief   Fills command buffer with command to get the checksum of a flash part
@@ -467,6 +471,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestCentroidTm();
   ReturnValue_t prepareRequestCentroidsTm();
   ReturnValue_t prepareRequestContrastTm();
+  ReturnValue_t prepareRequestBlobStatsTm();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();
@@ -550,6 +555,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
   void bootBootloader();
   bool reloadJsonCfgFile();
   ReturnValue_t acceptExternalDeviceCommands() override;
+
+  ReturnValue_t handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
+                                            startracker::FirmwareTarget target);
 };
 
 #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */

mission/acs/str/strHelpers.h

@@ -14,6 +14,12 @@ namespace startracker {
 static const Submode_t SUBMODE_BOOTLOADER = 1;
 static const Submode_t SUBMODE_FIRMWARE = 2;
 
+enum class FirmwareTarget : uint8_t { MAIN = 1, BACKUP = 10 };
+
+static constexpr char FW_TARGET_CFG_PATH[] = "startracker/fw-target.txt";
+
+enum ParamId : uint32_t { FIRMWARE_TARGET = 1, FIRMWARE_TARGET_PERSISTENT = 2 };
+
 class SdCardConfigPathGetter {
  public:
   virtual ~SdCardConfigPathGetter() = default;
@@ -322,6 +328,13 @@ enum PoolIds : lp_id_t {
   CONTRAST_B,
   CONTRAST_C,
   CONTRAST_D,
+
+  TICKS_BLOB_STATS,
+  TIME_BLOB_STATS,
+  BLOB_STATS_NOISE,
+  BLOB_STATS_THOLD,
+  BLOB_STATS_LVALID,
+  BLOB_STATS_OFLOW,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -373,7 +386,7 @@ static const DeviceCommandId_t REQ_DEBUG_CAMERA = 80;
 static const DeviceCommandId_t LOGLEVEL = 81;
 static const DeviceCommandId_t LOGSUBSCRIPTION = 82;
 static const DeviceCommandId_t DEBUG_CAMERA = 83;
-static const DeviceCommandId_t FIRMWARE_UPDATE = 84;
+static const DeviceCommandId_t FIRMWARE_UPDATE_MAIN = 84;
 static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
 static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
 static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
@@ -388,6 +401,9 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95;
 static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96;
 static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
 static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100;
+static constexpr DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101;
+static constexpr DeviceCommandId_t REQ_BLOB_STATS = 102;
+
 static const DeviceCommandId_t NONE = 0xFFFFFFFF;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
@@ -419,6 +435,7 @@ static const uint32_t BLOBS_SET_ID = REQ_BLOBS;
 static const uint32_t CENTROID_SET_ID = REQ_CENTROID;
 static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS;
 static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST;
+static const uint32_t BLOB_STATS_SET_ID = REQ_BLOB_STATS;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -485,11 +502,13 @@ static constexpr uint8_t CENTROID = 26;
 static constexpr uint8_t CENTROIDS = 37;
 static constexpr uint8_t AUTO_BLOB = 39;
 static constexpr uint8_t MATCHED_CENTROIDS = 40;
+static constexpr uint8_t BLOB_STATS = 49;
 }  // namespace ID
 
 namespace Program {
 static const uint8_t BOOTLOADER = 1;
-static const uint8_t FIRMWARE = 2;
+static const uint8_t FIRMWARE_MAIN = 2;
+static const uint8_t FIRMWARE_BACKUP = 3;
 }  // namespace Program
 
 namespace region_secrets {
@@ -509,7 +528,7 @@ static const uint32_t REGION_12_SECRET = 0x42fedef6;
 static const uint32_t REGION_13_SECRET = 0xe53cf10d;
 static const uint32_t REGION_14_SECRET = 0xe862b70b;
 static const uint32_t REGION_15_SECRET = 0x79b537ca;
-static const uint32_t secret[16]{
+static const uint32_t SECRETS[16]{
     REGION_0_SECRET,  REGION_1_SECRET,  REGION_2_SECRET,  REGION_3_SECRET,
     REGION_4_SECRET,  REGION_5_SECRET,  REGION_6_SECRET,  REGION_7_SECRET,
     REGION_8_SECRET,  REGION_9_SECRET,  REGION_10_SECRET, REGION_11_SECRET,
@@ -538,7 +557,12 @@ enum class FlashSections : uint8_t {
 };
 
 // Flash region IDs of firmware partition
-enum class FirmwareRegions : uint32_t { FIRST = 1, LAST = 8 };
+enum class FirmwareRegions : uint32_t {
+  FIRST_MAIN = 1,
+  LAST_MAIN = 8,
+  FIRST_BACKUP = 10,
+  LAST_BACKUP = 16
+};
 
 static const uint32_t FLASH_REGION_SIZE = 0x20000;
 
@@ -1545,6 +1569,23 @@ class CentroidsSet : public StaticLocalDataSet<10> {
       lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this);
 };
 
+class BlobStatsSet : public StaticLocalDataSet<6> {
+ public:
+  BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {}
+
+  // Data received from the Centroids Telemetry Set (ID 49)
+  lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_BLOB_STATS, this);
+  lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_BLOB_STATS, this);
+  lp_vec_t<uint8_t, 16> noise =
+      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_NOISE, this);
+  lp_vec_t<uint8_t, 16> thold =
+      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_THOLD, this);
+  lp_vec_t<uint8_t, 16> lvalid =
+      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_LVALID, this);
+  lp_vec_t<uint8_t, 16> oflow =
+      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this);
+};
+
 class ContrastSet : public StaticLocalDataSet<8> {
  public:
   ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}

mission/controller/AcsController.cpp

@@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
       sdcMan(sdcMan),
       attitudeEstimation(&acsParameters),
       fusedRotationEstimation(&acsParameters),
+      navigation(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       ptgCtrl(&acsParameters),
@@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
     }
     case UPDATE_TLE: {
       if (size != 69 * 2) {
-        return INVALID_PARAMETERS;
+        return HasActionsIF::INVALID_PARAMETERS;
       }
       ReturnValue_t result = updateTle(data, data + 69, false);
       if (result != returnvalue::OK) {
@@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
       }
       std::memcpy(tle + 69, line2, 69);
       actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
-      return EXECUTION_FINISHED;
+      return HasActionsIF::EXECUTION_FINISHED;
     }
+    case (UPDATE_MEKF_STANDARD_DEVIATIONS):
+      navigation.updateMekfStandardDeviations(&acsParameters);
+      return HasActionsIF::EXECUTION_FINISHED;
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -173,10 +177,10 @@ void AcsController::performAttitudeControl() {
                            &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
   fusedRotationEstimation.estimateFusedRotationRate(
-      &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
+      mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
       &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &attitudeEstimationData, &acsParameters);
+                              &susDataProcessed, timeDelta, &attitudeEstimationData);
 
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
@@ -396,7 +400,7 @@ void AcsController::performPointingCtrl() {
 
   bool allRwAvailable = true;
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail);
   if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
     if (multipleRwUnavailableCounter >=
         acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@@ -433,10 +437,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
+          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       break;
 
     case acs::PTG_TARGET:
@@ -456,10 +460,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
+          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       break;
 
     case acs::PTG_TARGET_GS:
@@ -476,10 +480,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
+          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       break;
 
     case acs::PTG_NADIR:
@@ -498,10 +502,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
+          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       break;
 
     case acs::PTG_INERTIAL:
@@ -520,10 +524,10 @@ void AcsController::performPointingCtrl() {
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
+          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       break;
     default:
       sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;

mission/controller/AcsController.h

@@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipoleMtqs[3] = {0, 0, 0};
 
+  acsctrl::RwAvail rwAvail;
+
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif
@@ -111,6 +113,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
   static const DeviceCommandId_t UPDATE_TLE = 0x3;
   static const DeviceCommandId_t READ_TLE = 0x4;
+  static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
 
   ReturnValue_t initialize() override;
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;

mission/controller/acs/AcsParameters.cpp

@@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
     case (0x11):  // KalmanFilterParameters
       switch (parameterId) {
         case 0x0:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
           break;
         case 0x1:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
           break;
         case 0x2:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
           break;
         case 0x3:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
           break;
         case 0x4:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
           break;
         case 0x5:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
           break;
         default:
           return INVALID_IDENTIFIER_ID;

mission/controller/acs/AcsParameters.h

@@ -8,6 +8,9 @@
 typedef unsigned char uint8_t;
 
 class AcsParameters : public HasParametersIF {
+ private:
+  static constexpr double DEG2RAD = M_PI / 180.;
+
  public:
   AcsParameters();
   virtual ~AcsParameters();
@@ -20,9 +23,9 @@ class AcsParameters : public HasParametersIF {
     double sampleTime = 0.4;  // [s]
     uint16_t ptgCtrlLostTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
-    uint8_t fusedRateFromStr = false;
-    uint8_t fusedRateFromQuest = false;
-    double questFilterWeight = 0.0;
+    uint8_t fusedRateFromStr = true;
+    uint8_t fusedRateFromQuest = true;
+    double questFilterWeight = 0.9;
   } onBoardParams;
 
   struct InertiaEIVE {
@@ -773,7 +776,7 @@ class AcsParameters : public HasParametersIF {
          0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
          -0.000889232196185857, -0.00168429567131815}};
     float susBrightnessThreshold = 0.7;
-    float susVectorFilterWeight = .85;
+    float susVectorFilterWeight = .95;
     float susRateFilterWeight = .99;
   } susHandlingParameters;
 
@@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF {
   struct PointingLawParameters {
     double zeta = 0.3;
     double om = 0.3;
-    double omMax = 1 * M_PI / 180;
+    double omMax = 1 * DEG2RAD;
     double qiMin = 0.1;
 
     double gainNullspace = 0.01;
@@ -876,15 +879,15 @@ class AcsParameters : public HasParametersIF {
     uint8_t timeElapsedMax = 10;  // rot rate calculations
 
     // Default is Stuttgart GS
-    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
-    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
-    double altitudeTgt = 500;                     // [m]
+    double latitudeTgt = 48.7495 * DEG2RAD;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * DEG2RAD;  // [rad] Longitude
+    double altitudeTgt = 500;                 // [m]
 
     // For one-axis control:
     uint8_t avoidBlindStr = true;
     double blindAvoidStart = 1.5;
     double blindAvoidStop = 2.5;
-    double blindRotRate = 1 * M_PI / 180;
+    double blindRotRate = 1. * DEG2RAD;
   } targetModeControllerParameters;
 
   struct GsTargetModeControllerParameters : PointingLawParameters {
@@ -892,9 +895,9 @@ class AcsParameters : public HasParametersIF {
     uint8_t timeElapsedMax = 10;          // rot rate calculations
 
     // Default is Stuttgart GS
-    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
-    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
-    double altitudeTgt = 500;                     // [m]
+    double latitudeTgt = 48.7495 * DEG2RAD;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * DEG2RAD;  // [rad] Longitude
+    double altitudeTgt = 500;                 // [m]
   } gsTargetModeControllerParameters;
 
   struct NadirModeControllerParameters : PointingLawParameters {
@@ -911,8 +914,8 @@ class AcsParameters : public HasParametersIF {
   } inertialModeControllerParameters;
 
   struct StrParameters {
-    double exclusionAngle = 20 * M_PI / 180;
-    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // geometry frame
+    double exclusionAngle = 20. * DEG2RAD;
+    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // body rf
   } strParameters;
 
   struct GpsParameters {
@@ -925,25 +928,25 @@ class AcsParameters : public HasParametersIF {
 
   struct SunModelParameters {
     float domega = 36000.771;
-    float omega_0 = 280.46 * M_PI / 180.;  // RAAN plus argument of
-                                           // perigee
-    float m_0 = 357.5277;                  // coefficients for mean anomaly
-    float dm = 35999.049;                  // coefficients for mean anomaly
-    float e = 23.4392911 * M_PI / 180.;    // angle of earth's rotation axis
-    float e1 = 0.74508 * M_PI / 180.;
+    float omega_0 = 280.46 * DEG2RAD;  // RAAN plus argument of
+                                       // perigee
+    float m_0 = 357.5277;              // coefficients for mean anomaly
+    float dm = 35999.049;              // coefficients for mean anomaly
+    float e = 23.4392911 * DEG2RAD;    // angle of earth's rotation axis
+    float e1 = 0.74508 * DEG2RAD;
 
-    float p1 = 6892. / 3600. * M_PI / 180.;  // some parameter
-    float p2 = 72. / 3600. * M_PI / 180.;    // some parameter
+    float p1 = 6892. / 3600. * DEG2RAD;  // some parameter
+    float p2 = 72. / 3600. * DEG2RAD;    // some parameter
   } sunModelParameters;
 
   struct KalmanFilterParameters {
-    double sensorNoiseSTR = 0.1 * M_PI / 180;
-    double sensorNoiseSS = 8 * M_PI / 180;
-    double sensorNoiseMAG = 4 * M_PI / 180;
-    double sensorNoiseGYR = 0.1 * M_PI / 180;
+    double sensorNoiseStr = 0.1 * DEG2RAD;
+    double sensorNoiseSus = 8. * DEG2RAD;
+    double sensorNoiseMgm = 4. * DEG2RAD;
+    double sensorNoiseGyr = 0.1 * DEG2RAD;
 
-    double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180;  // Angular Random Walk
-    double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600;                // Bias Stability
+    double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD;  // Angular Random Walk
+    double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD;               // Bias Stability
   } kalmanFilterParameters;
 
   struct MagnetorquerParameter {
@@ -959,8 +962,8 @@ class AcsParameters : public HasParametersIF {
 
   struct DetumbleParameter {
     uint8_t detumblecounter = 75;  // 30 s
-    double omegaDetumbleStart = 2 * M_PI / 180;
-    double omegaDetumbleEnd = 1 * M_PI / 180;
+    double omegaDetumbleStart = 2 * DEG2RAD;
+    double omegaDetumbleEnd = 1 * DEG2RAD;
     double gainBdot = pow(10.0, -3.3);
     double gainFull = pow(10.0, -2.3);
     uint8_t useFullDetumbleLaw = false;

mission/controller/acs/AttitudeEstimation.cpp

@@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
 
   // Sensor Weights
   double kSus = 0, kMgm = 0;
-  kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
-  kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
+  kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
+  kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
 
   // Weighted Vectors
   double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},

mission/controller/acs/FusedRotationEstimation.cpp

@@ -5,18 +5,18 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
 }
 
 void FusedRotationEstimation::estimateFusedRotationRate(
-    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
-    acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
-    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
+    const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
+    acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
+    ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData,
+    const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
     acsctrl::FusedRotRateData *fusedRotRateData) {
   estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
   estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
   estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
                                   fusedRotRateSourcesData);
 
-  if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
-      acsParameters->onBoardParams.fusedRateFromStr) {
+  if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
+                                           acsParameters->onBoardParams.fusedRateFromStr)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
@@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
       fusedRotRateData->rotRateSource.setValid(true);
     }
-  } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
-             acsParameters->onBoardParams.fusedRateFromQuest) {
+  } else if (not(mode == acs::AcsMode::SAFE) and
+             (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
+              acsParameters->onBoardParams.fusedRateFromQuest)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));

mission/controller/acs/FusedRotationEstimation.h

@@ -12,7 +12,7 @@ class FusedRotationEstimation {
  public:
   FusedRotationEstimation(AcsParameters *acsParameters_);
 
-  void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
+  void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
                                  ACS::SensorValues *sensorValues,

mission/controller/acs/Guidance.cpp

@@ -8,8 +8,8 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
                                  const double sunDirI[3], const double posSatF[4],
                                  double targetQuat[4], double targetSatRotRate[3]) {
   // positive z-Axis of EIVE in direction of sun
-  double zAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(sunDirI, zAxisXI, 3);
+  double zAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);
 
   // determine helper vector to point x-Axis and therefore the STR away from Earth
   double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
@@ -17,39 +17,37 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
   VectorOperations<double>::normalize(posSatI, helperXI, 3);
 
   // construct y-axis from helper vector and z-axis
-  double yAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxisXI, helperXI, yAxisXI);
-  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
+  double yAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
+  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
 
   // x-axis completes RHS
-  double xAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(yAxisXI, zAxisXI, xAxisXI);
-  VectorOperations<double>::normalize(xAxisXI, xAxisXI, 3);
+  double xAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
+  VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);
 
   // join transformation matrix
-  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
-                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
-                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
-  QuaternionOperations::fromDcm(dcmXI, targetQuat);
+  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
+                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
+                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
+  QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
   // calculate of reference rotation rate
   targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
-                                   double velSatF[3], double targetQuat[4],
-                                   double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
+                                   const double posSatF[3], const double velSatF[3],
+                                   double targetQuat[4], double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for target pointing
   //-------------------------------------------------------------------------------------
   // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
   double targetF[3] = {0, 0, 0};
-  MathOperations<double>::cartesianFromLatLongAlt(
+  CoordinateTransformations::cartesianFromLatLongAlt(
       acsParameters->targetModeControllerParameters.latitudeTgt,
       acsParameters->targetModeControllerParameters.longitudeTgt,
       acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
-  double targetDirF[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
 
   // target direction in the ECI frame
   double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
@@ -59,8 +57,8 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
 
   // x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
-  double xAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirI, xAxisXI, 3);
+  double xAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
 
   // transform velocity into inertial frame
   double velSatI[3] = {0, 0, 0};
@@ -73,32 +71,32 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
 
   // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
   // resolution
-  double yAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(orbitalNormalI, xAxisXI, yAxisXI);
-  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
+  double yAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
+  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
 
   // z-axis completes RHS
-  double zAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(xAxisXI, yAxisXI, zAxisXI);
+  double zAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
 
   // join transformation matrix
-  double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
-                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
-                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
+  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
+                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
+                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
   QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
   targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
-                               double sunDirI[3], double targetQuat[4],
-                               double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
+                               const double posSatF[3], const double sunDirI[3],
+                               double targetQuat[4], double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for ground station pointing
   //-------------------------------------------------------------------------------------
   // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
   double posGroundStationF[3] = {0, 0, 0};
-  MathOperations<double>::cartesianFromLatLongAlt(
+  CoordinateTransformations::cartesianFromLatLongAlt(
       acsParameters->gsTargetModeControllerParameters.latitudeTgt,
       acsParameters->gsTargetModeControllerParameters.longitudeTgt,
       acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
@@ -106,43 +104,93 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou
   // target direction in the ECI frame
   double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
   CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
-  CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
+  CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
   VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
 
   // negative x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
-  double xAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(groundStationDirI, xAxisXI, 3);
-  VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
+  double xAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
+  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
 
-  // get sun vector model in ECI
-  VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
+  // get earth vector in ECI
+  double earthDirI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(posSatI, earthDirI, 3);
+  VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
 
-  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
-  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
-  double xDotS = VectorOperations<double>::dot(xAxisXI, sunDirI);
-  xDotS /= pow(VectorOperations<double>::norm(xAxisXI, 3), 2);
-  double sunParallel[3], zAxisXI[3];
-  VectorOperations<double>::mulScalar(xAxisXI, xDotS, sunParallel, 3);
-  VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisXI, 3);
-  VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
+  // sun avoidance calculations
+  double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
+                                      sunPerpendicularX, 3);
+  VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
+  VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
+  VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
+  double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
+         strVecSunZ[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
+                                      strVecSunX, 3);
+  VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
+                                      strVecSunZ, 3);
+  VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
+  VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
+  sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
 
-  // y-axis completes RHS
-  double yAxisXI[3];
-  VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
-  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
+  // earth avoidance calculations
+  double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
+                                      earthPerpendicularX, 3);
+  VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
+  VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
+  VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
+  double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
+         strVecEarthZ[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
+                                      strVecEarthX, 3);
+  VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
+                                      strVecEarthZ, 3);
+  VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
+  VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
+  earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
+
+  if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
+    // if this actually ever happens i will eat a broom
+    sunWeight = 0.5;
+    earthWeight = 0.5;
+  }
+
+  // normalize weights for convenience
+  double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
+  sunWeight *= normFactor;
+  earthWeight *= normFactor;
+
+  // calculate z-axis for str blinding avoidance
+  double zAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
+  VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
+  VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
+  VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
+  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
+
+  // calculate y-axis
+  double yAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
+  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
 
   // join transformation matrix
-  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
-                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
-                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
-  QuaternionOperations::fromDcm(dcmXI, targetQuat);
+  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
+                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
+                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
+  QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
+  limitReferenceRotation(xAxisIX, targetQuat);
   targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
+
+  std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
 }
 
-void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
-                                  double velSatE[3], double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
+                                  const double posSatE[3], const double velSatE[3],
+                                  double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
   //-------------------------------------------------------------------------------------
@@ -153,26 +201,26 @@ void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
 
   // negative x-axis aligned with position vector
   // this aligns with the camera, E- and S-band antennas
-  double xAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(posSatI, xAxisXI, 3);
-  VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
+  double xAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
+  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
 
   // make z-Axis parallel to major part of camera resolution
-  double zAxisXI[3] = {0, 0, 0};
+  double zAxisIX[3] = {0, 0, 0};
   double velSatI[3] = {0, 0, 0};
   CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
-  VectorOperations<double>::cross(xAxisXI, velSatI, zAxisXI);
-  VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
+  VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
+  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
 
   // y-Axis completes RHS
-  double yAxisXI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
+  double yAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
 
   // join transformation matrix
-  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
-                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
-                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
-  QuaternionOperations::fromDcm(dcmXI, targetQuat);
+  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
+                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
+                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
+  QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
   targetRotationRate(timeDelta, targetQuat, refSatRate);
 }
@@ -189,6 +237,63 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
   std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
 }
 
+void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
+  if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
+      (VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
+    return;
+  }
+
+  // check required rotation and return if below limit
+  double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
+  QuaternionOperations::inverse(quatIXprev, quatXprevI);
+  QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
+  QuaternionOperations::normalize(quatXprevX);
+  double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
+                  acsParameters->onBoardParams.sampleTime;
+  if (2 * std::acos(quatXprevX[3]) < phiMax) {
+    return;
+  }
+
+  // x-axis always needs full rotation
+  double phiX = 0, phiXvec[3] = {0, 0, 0};
+  phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
+  VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
+  VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
+
+  double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
+  std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
+  quatXprevXtilde[3] = cos(phiX / 2.);
+  QuaternionOperations::normalize(quatXprevXtilde);
+  QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
+
+  // use the residual rotation up to the maximum
+  double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
+  QuaternionOperations::inverse(quatIX, quatXI);
+  QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
+
+  double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
+  if ((phiX * phiX) > (phiMax * phiMax)) {
+    phiResidual = 0;
+  } else {
+    phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
+  }
+  std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
+  VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
+
+  double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
+                                      3);
+  std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
+  quatXhatXTilde[3] = std::cos(phiResidual / 2.);
+  QuaternionOperations::normalize(quatXhatXTilde);
+
+  // calculate final quaternion
+  QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
+  QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
+  QuaternionOperations::normalize(quatIX);
+}
+
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                           double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
@@ -226,28 +331,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
-                                                double *rwPseudoInv) {
-  bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
-  bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
-  bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
-  bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
+                                                double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
+  rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
+  rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
+  rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
+  rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
 
-  if (rw1valid and rw2valid and rw3valid and rw4valid) {
+  if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
+  } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
+  } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
+  } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
+             rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
+  } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
+             not rwAvail->rw4avail) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
                 12 * sizeof(double));
     return acsctrl::SINGLE_RW_UNAVAILABLE;
@@ -255,7 +364,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
   return acsctrl::MULTIPLE_RW_UNAVAILABLE;
 }
 
-void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
+void Guidance::resetValues() {
+  std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
+  std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
+}
 
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
   std::error_code e;

mission/controller/acs/Guidance.h

@@ -8,9 +8,7 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/SensorValues.h>
-#include <mission/controller/acs/util/MathOperations.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
-#include <time.h>
 
 #include <cmath>
 #include <filesystem>
@@ -27,16 +25,18 @@ class Guidance {
 
   void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
                          const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
-  void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
-                           double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
-                       double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
-  void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
-                          double velSatF[3], double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
+                           const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
+  void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
+                       const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
+  void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
+                          const double velSatF[3], double targetQuat[4], double refSatRate[3]);
 
   void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
                           double *targetSatRotRate);
 
+  void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
+
   void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                   double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                   double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
@@ -44,7 +44,8 @@ class Guidance {
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                   double &errorAngle);
 
-  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
+  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
+                                        acsctrl::RwAvail *rwAvail);
 
  private:
   const AcsParameters *acsParameters;
@@ -54,6 +55,7 @@ class Guidance {
 
   bool strBlindAvoidFlag = false;
   double quatIXprev[4] = {0, 0, 0, 0};
+  double xAxisIXprev[3] = {0, 0, 0};
 
   static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
   static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";

mission/controller/acs/Igrf13Model.cpp

@@ -1,19 +1,5 @@
 #include "Igrf13Model.h"
 
-#include <fsfw/src/fsfw/globalfunctions/constants.h>
-#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
-#include <stdint.h>
-#include <string.h>
-#include <time.h>
-
-#include <cmath>
-
-#include "util/MathOperations.h"
-
-using namespace Math;
-
 Igrf13Model::Igrf13Model() {}
 Igrf13Model::~Igrf13Model() {}
 
@@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
   double magFieldModel[3] = {0, 0, 0};
   double phi = longitude, theta = gcLatitude;  // geocentric
   /* Here is the co-latitude needed*/
-  theta -= 90 * PI / 180;
+  theta -= 90. * M_PI / 180.;
   theta *= (-1);
 
   double rE = 6371200.0;  // radius earth [m]
@@ -83,13 +69,14 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
   magFieldModel[1] *= -1;
   magFieldModel[2] *= (-1 / sin(theta));
 
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
+  double JD2000 = 0;
+  Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
   double UT1 = JD2000 / 36525.;
 
   double gst =
       280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
   gst = std::fmod(gst, 360.);
-  gst *= PI / 180.;
+  gst *= M_PI / 180.;
   double lst = gst + longitude;  // local sidereal time [rad]
 
   magFieldModelInertial[0] =
@@ -107,7 +94,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
 
 void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
   double JD2000Igrf = (2458850.0 - 2451545);  // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
+  double JD2000 = 0;
+  Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
   double days = ceil(JD2000 - JD2000Igrf);
   for (int i = 0; i <= igrfOrder; i++) {
     for (int j = 0; j <= (igrfOrder - 1); j++) {

mission/controller/acs/Igrf13Model.h

@@ -16,10 +16,11 @@
 #ifndef IGRF13MODEL_H_
 #define IGRF13MODEL_H_
 
-#include <fsfw/parameters/HasParametersIF.h>
-#include <stdint.h>
-#include <string.h>
-#include <time.h>
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/src/fsfw/timemanager/Clock.h>
 
 #include <cmath>
 

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -1,14 +1,16 @@
 #ifndef MULTIPLICATIVEKALMANFILTER_H_
 #define MULTIPLICATIVEKALMANFILTER_H_
 
-#include <stdint.h>
-
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "AcsParameters.h"
-#include "eive/resultClassIds.h"
+#include <common/config/eive/resultClassIds.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class MultiplicativeKalmanFilter {
-  /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
+  /* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by
    *           means of the spacecraft attitude sensors
    *
    * @note: A description of the used algorithms can be found in the bachelor thesis of Robin
@@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter {
  public:
   /* @brief: Constructor
    */
-  MultiplicativeKalmanFilter();
+  MultiplicativeKalmanFilter(AcsParameters *acsParameters);
   virtual ~MultiplicativeKalmanFilter();
 
-  ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
+  ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  /* @brief: init() - This function initializes the Kalman Filter and will provide the first
-   * quaternion through the QUEST algorithm
-   * @param:  magneticField_  magnetic field vector in the body frame
-   * 		  sunDir_ 		  sun direction vector in the body frame
-   * 		  sunDirJ 		  sun direction vector in the ECI frame
-   * 		  magFieldJ 	  magnetic field vector in the ECI frame
-   */
-  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
-                     const bool validSS, const double *sunDirJ, const bool validSSModel,
-                     const double *magFieldJ, const bool validMagModel,
-                     acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
+  ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
+                     const acsctrl::MgmDataProcessed *mgmData,
+                     const acsctrl::GyrDataProcessed *gyrData,
+                     acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
-   * for the current step after the initalization
-   * @param:  quaternionSTR   Star Tracker Quaternion between from body to ECI frame
-   * 			rateGYRs_ 		Estimated satellite rotation rate from the
-   * Gyroscopes [rad/s] magneticField_  magnetic field vector in the body frame sunDir_
-   * sun direction vector in the body frame sunDirJ 		sun direction vector in the ECI
-   * frame magFieldJ 		magnetic field vector in the ECI frame
-   * 			outputQuat 		Stores the calculated quaternion
-   * 			outputSatRate 	Stores the adjusted satellite rate
-   * @return 	ReturnValue_t	Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
-   * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
-   * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
-   * 							RETURN_OK else
-   */
-  ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
-                        const bool validGYRs_, const double *magneticField_,
-                        const bool validMagField_, const double *sunDir_, const bool validSS,
-                        const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
-                        AcsParameters *acsParameters);
+  ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
+                        const acsctrl::MgmDataProcessed *mgmData,
+                        const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
+                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  enum MekfStatus : uint8_t {
-    UNINITIALIZED = 0,
-    NO_GYR_DATA = 1,
-    NO_MODEL_VECTORS = 2,
-    NO_SUS_MGM_STR_DATA = 3,
-    COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
-    INITIALIZED = 10,
-    RUNNING = 11,
-  };
+  void updateStandardDeviations(const AcsParameters *acsParameters);
+
+  void setStrData(const double qX, const double qY, const double qZ, const double qW,
+                  const bool valid);
 
-  // resetting Mekf
   static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
   static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
   static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
@@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter {
  private:
   static constexpr double ZERO_VEC3[3] = {0, 0, 0};
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+  static constexpr double ZERO_MAT66[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                              {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                              {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
+  static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
+                                        {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
 
-  /*Parameters*/
-  double quaternion_STR_SB[4];
+  enum MekfStatus : uint8_t {
+    UNINITIALIZED = 0,
+    NO_GYR_DATA = 1,
+    NO_MODEL_VECTORS = 2,
+    NO_SUS_MGM_STR_DATA = 3,
+    COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
+    INITIALIZED = 10,
+    RUNNING = 11,
+  };
 
-  /*States*/
-  double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
-  double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
-                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
-                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
-  double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
-  uint8_t sensorsAvail = 0;
+  enum SensorAvailability : uint8_t {
+    NONE = 0,
+    SUS_MGM_STR = 1,
+    SUS_MGM = 2,
+    SUS_STR = 3,
+    MGM_STR = 4,
+    SUS = 5,
+    MGM = 6,
+    STR = 7,
+  };
 
-  /*Outputs*/
-  double quatBJ[4];  /* Output Quaternion */
-  double biasGYR[3]; /*Between measured and estimated sat Rate*/
-                     /*Parameter INIT*/
-  /*Functions*/
-  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
-  void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
-                     double quat[4], double satRotRate[3]);
+  MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
+
+  struct StrData {
+    struct StrQuat {
+      double value[4] = {0, 0, 0, 0};
+      bool valid = false;
+    } strQuat;
+  } strData;
+
+  // Standard Deviations
+  double sigmaSus = 0;
+  double sigmaMgm = 0;
+  double sigmaStr = 0;
+  double sigmaGyr = 0;
+  // sigmaV
+  double sigmaGyrArw = 0;
+  // sigmaU
+  double sigmaGyrBs = 0;
+
+  // Covariance Matrices
+  double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+  double covAposteriori[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+
+  // Sensor Availability
+  SensorAvailability sensorsAvailable = SensorAvailability::NONE;
+  uint8_t matrixDimensionFactor = 0;
+
+  // Estimated States
+  double estimatedQuaternionBI[4] = {0, 0, 0, 1};
+  double estimatedBiasGyr[3] = {0, 0, 0};
+  double estimatedRotRate[3] = {0, 0, 0};
+  double estimatedCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+
+  // Functions
+  ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
+                                      const acsctrl::MgmDataProcessed *mgmData,
+                                      const acsctrl::GyrDataProcessed *gyrData,
+                                      acsctrl::AttitudeEstimationData *attitudeEstimationData);
+
+  void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
+                double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
+  ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
+                       acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
+  void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
+  void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
+
+  ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+
+  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
 };
 
 #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

mission/controller/acs/Navigation.cpp

@@ -1,43 +1,27 @@
 #include "Navigation.h"
 
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>
-
-#include "util/CholeskyDecomposition.h"
-#include "util/MathOperations.h"
-
-Navigation::Navigation() {}
+Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
 
 Navigation::~Navigation() {}
 
-ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
-                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
-                                  acsctrl::SusDataProcessed *susDataProcessed,
-                                  acsctrl::AttitudeEstimationData *mekfData,
-                                  AcsParameters *acsParameters) {
-  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
-                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
+ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
+                                  const acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                  const acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                  const acsctrl::SusDataProcessed *susDataProcessed,
+                                  const double timeDelta,
+                                  acsctrl::AttitudeEstimationData *attitudeEstimationData) {
+  multiplicativeKalmanFilter.setStrData(
+      sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
+      sensorValues->strSet.caliQx.isValid());
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
-    mekfStatus = multiplicativeKalmanFilter.init(
-        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
-        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
-        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
-        acsParameters);
+    mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
+                                                 gyrDataProcessed, attitudeEstimationData);
     return mekfStatus;
   } else {
     mekfStatus = multiplicativeKalmanFilter.mekfEst(
-        quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
-        gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
-        mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
-        susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
-        susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
+        susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
     return mekfStatus;
   }
 }
@@ -82,3 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
 ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
   return sgp4Propagator.initialize(line1, line2);
 }
+
+void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
+  multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
+}

mission/controller/acs/Navigation.h

@@ -5,24 +5,24 @@
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
-#include <mission/controller/acs/SensorProcessing.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class Navigation {
  public:
-  Navigation();
+  Navigation(AcsParameters *acsParameters);
   virtual ~Navigation();
 
-  ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
-                        acsctrl::GyrDataProcessed *gyrDataProcessed,
-                        acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        acsctrl::SusDataProcessed *susDataProcessed,
-                        acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
+  ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
+                        const acsctrl::GyrDataProcessed *gyrDataProcessed,
+                        const acsctrl::MgmDataProcessed *mgmDataProcessed,
+                        const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
+                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
   void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
   ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
+  void updateMekfStandardDeviations(const AcsParameters *acsParameters);
 
  protected:
  private:

mission/controller/acs/SensorProcessing.cpp

@@ -180,7 +180,8 @@ void SensorProcessing::processSus(
     const AcsParameters::SunModelParameters *sunModelParameters,
     acsctrl::SusDataProcessed *susDataProcessed) {
   /* -------- Sun Model Direction (IJK frame) ------- */
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
+  double JD2000 = 0;
+  Clock::convertTimevalToJD2000(timeAbsolute, &JD2000);
 
   // Julean Centuries
   double sunIjkModel[3] = {0.0, 0.0, 0.0};
@@ -198,6 +199,7 @@ void SensorProcessing::processSus(
   sunIjkModel[0] = cos(eclipticLongitude);
   sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
   sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
+  VectorOperations<double>::normalize(sunIjkModel, sunIjkModel, 3);
 
   uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   if (sus0valid) {
@@ -528,8 +530,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
   uint8_t gpsSource = acs::gps::Source::NONE;
   // We do not trust the GPS and therefore it shall die here if SPG4 is running
   if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
-    MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
-                                                    gdLongitude, altitude);
+    CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
+                                                       gdLatitude, gdLongitude, altitude);
     double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
     gcLatitude = atan(factor * tan(gdLatitude));
     {
@@ -559,7 +561,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
 
     // Calculation of the satellite velocity in earth fixed frame
     double deltaDistance[3] = {0, 0, 0};
-    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
+    CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
     if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
       VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
       VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);

mission/controller/acs/SensorProcessing.h

@@ -2,6 +2,7 @@
 #define SENSORPROCESSING_H_
 
 #include <common/config/eive/resultClassIds.h>
+#include <fsfw/coordinates/CoordinateTransformations.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
@@ -9,12 +10,12 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/globalfunctions/timevalOperations.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw/timemanager/Clock.h>
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/Igrf13Model.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/acs/SusConverter.h>
-#include <mission/controller/acs/util/MathOperations.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 #include <cmath>

mission/controller/acs/control/PtgCtrl.cpp

@@ -1,11 +1,5 @@
 #include "PtgCtrl.h"
 
-#include <fsfw/globalfunctions/constants.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <fsfw/globalfunctions/sign.h>
-
 PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 PtgCtrl::~PtgCtrl() {}
@@ -15,10 +9,10 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
     const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
   if (not magFieldValid) {
     return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
-  } else if (mekfEnabled and mekfValid) {
-    return acs::ControlModeStrategy::PTGCTRL_MEKF;
   } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
     return acs::ControlModeStrategy::PTGCTRL_STR;
+  } else if (mekfEnabled and mekfValid) {
+    return acs::ControlModeStrategy::PTGCTRL_MEKF;
   } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
     return acs::ControlModeStrategy::PTGCTRL_QUEST;
   }
@@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
 
   // Inverse of gainMatrix
   double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
-  gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
-  gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
+  gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
+  gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
+  gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
 
   double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MatrixOperations<double>::multiply(
@@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
                                       4);
 }
 
-void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail,
+                              AcsParameters::PointingLawParameters *pointingLawParameters,
                               const double *magFieldB, const bool magFieldBValid,
-                              const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
+                              const int32_t speedRw0, const int32_t speedRw1,
                               const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
   if (not magFieldBValid or not pointingLawParameters->desatOn) {
     return;
@@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  // calculate angular momentum of the satellite
-  double angMomentumSat[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
-                                     angMomentumSat, 3, 3, 1);
-
   // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
   // relocate RW speed zero to nullspace RW speed
   double refSpeedRws[4] = {0, 0, 0, 0};
   VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
                                       pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
+  if (not allRwAvailable) {
+    if (not rwAvail->rw1avail) {
+      refSpeedRws[0] = 0.0;
+    } else if (not rwAvail->rw2avail) {
+      refSpeedRws[1] = 0.0;
+    } else if (not rwAvail->rw3avail) {
+      refSpeedRws[2] = 0.0;
+    } else if (not rwAvail->rw4avail) {
+      refSpeedRws[3] = 0.0;
+    }
+  }
   VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
+
   // convert speed from 10 RPM to 1 RPM
   VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
   // convert to rad/s
@@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
 
   // calculate total angular momentum
   double angMomentumTotal[3] = {0, 0, 0};
-  VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
-
-  // calculating momentum error
-  double deltaAngMomentum[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
-                                     deltaAngMomentum, 3);
+  VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
+                                     angMomentumTotal, 3);
 
   // resulting magnetic dipole command
   double crossAngMomentumMagField[3] = {0, 0, 0};
-  VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
+  VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
   double factor =
       pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
   VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
@@ -221,6 +219,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
             rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
           } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
             rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
+          } else {
+            rwCmdSpeeds[i] = 0;
           }
         }
       }

mission/controller/acs/control/PtgCtrl.h

@@ -1,11 +1,16 @@
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_
 
-#include <math.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/globalfunctions/sign.h>
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/SensorValues.h>
-#include <stdio.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+
+#include <cmath>
 
 class PtgCtrl {
   /*
@@ -38,10 +43,11 @@ class PtgCtrl {
                     const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                     const int32_t speedRw3, double *rwTrqNs);
 
-  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
-                       const double *magFieldB, const bool magFieldBValid, const double *satRate,
-                       const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
-                       const int32_t speedRw3, double *mgtDpDes);
+  void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
+                       AcsParameters::PointingLawParameters *pointingLawParameters,
+                       const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
+                       const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
+                       double *mgtDpDes);
 
   /* @brief: Commands the stiction torque in case wheel speed is to low
    * 		 torqueCommand  modified torque after anti-stiction

mission/controller/acs/util/CholeskyDecomposition.h

@@ -1,98 +0,0 @@
-/*
- * TinyEKF: Extended Kalman Filter for embedded processors
- *
- * Copyright (C) 2015 Simon D. Levy
- *
- * MIT License
- */
-#ifndef CHOLESKYDECOMPOSITION_H_
-#define CHOLESKYDECOMPOSITION_H_
-#include <math.h>
-// typedef unsigned int uint8_t;
-
-template <typename T1, typename T2 = T1, typename T3 = T2>
-class CholeskyDecomposition {
- public:
-  static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
-    // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-    return cholsl(matrix, result, tempMatrix, dimension);
-  }
-
- private:
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t choldc1(double *a, double *p, uint8_t n) {
-    int8_t i, j, k;
-    double sum;
-
-    for (i = 0; i < n; i++) {
-      for (j = i; j < n; j++) {
-        sum = a[i * n + j];
-        for (k = i - 1; k >= 0; k--) {
-          sum -= a[i * n + k] * a[j * n + k];
-        }
-        if (i == j) {
-          if (sum <= 0) {
-            return 1; /* error */
-          }
-          p[i] = sqrt(sum);
-        } else {
-          a[j * n + i] = sum / p[i];
-        }
-      }
-    }
-
-    return 0; /* success */
-  }
-
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
-    uint8_t i, j, k;
-    double sum;
-    for (i = 0; i < n; i++)
-      for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
-    if (choldc1(a, p, n)) return 1;
-    for (i = 0; i < n; i++) {
-      a[i * n + i] = 1 / p[i];
-      for (j = i + 1; j < n; j++) {
-        sum = 0;
-        for (k = i; k < j; k++) {
-          sum -= a[j * n + k] * a[k * n + i];
-        }
-        a[j * n + i] = sum / p[j];
-      }
-    }
-
-    return 0; /* success */
-  }
-
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
-    uint8_t i, j, k;
-    if (choldcsl(A, a, p, n)) return 1;
-    for (i = 0; i < n; i++) {
-      for (j = i + 1; j < n; j++) {
-        a[i * n + j] = 0.0;
-      }
-    }
-    for (i = 0; i < n; i++) {
-      a[i * n + i] *= a[i * n + i];
-      for (k = i + 1; k < n; k++) {
-        a[i * n + i] += a[k * n + i] * a[k * n + i];
-      }
-      for (j = i + 1; j < n; j++) {
-        for (k = j; k < n; k++) {
-          a[i * n + j] += a[k * n + i] * a[k * n + j];
-        }
-      }
-    }
-    for (i = 0; i < n; i++) {
-      for (j = 0; j < i; j++) {
-        a[i * n + j] = a[j * n + i];
-      }
-    }
-
-    return 0; /* success */
-  }
-};
-
-#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */

mission/controller/acs/util/MathOperations.h

@@ -1,465 +0,0 @@
-#ifndef MATH_MATHOPERATIONS_H_
-#define MATH_MATHOPERATIONS_H_
-
-#include <fsfw/src/fsfw/globalfunctions/constants.h>
-#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/src/fsfw/globalfunctions/sign.h>
-#include <fsfw/src/fsfw/serviceinterface.h>
-
-#include <cmath>
-#include <cstring>
-#include <iostream>
-
-template <typename T1, typename T2 = T1>
-class MathOperations {
- public:
-  static void skewMatrix(const T1 vector[], T2 *result) {
-    // Input Dimension [3], Output [3][3]
-    result[0] = 0;
-    result[1] = -vector[2];
-    result[2] = vector[1];
-    result[3] = vector[2];
-    result[4] = 0;
-    result[5] = -vector[0];
-    result[6] = -vector[1];
-    result[7] = vector[0];
-    result[8] = 0;
-  }
-  static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
-                                    uint8_t size = 3) {
-    // Looks like MatrixOpertions::multiply is able to do the same thing
-    for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
-      for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
-        result[resultColumn + size * resultRow] =
-            vector1[resultRow] * transposeVector2[resultColumn];
-      }
-    }
-    /*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
-     matrixMag[i][j] = magEstB[i] * magEstB[j];
-     matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
-     matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
-  }
-
-  static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
-    int min_idx;
-    T1 temp;
-    std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
-    // One by one move boundary of unsorted subarray
-    for (int k = 0; k < rowSize; k++) {
-      for (int i = 0; i < colSize - 1; i++) {
-        // Find the minimum element in unsorted array
-        min_idx = i;
-        for (int j = i + 1; j < colSize; j++) {
-          if (result[j + k * colSize] < result[min_idx + k * colSize]) {
-            min_idx = j;
-          }
-        }
-        // Swap the found minimum element with the first element
-        temp = result[i + k * colSize];
-        result[i + k * colSize] = result[min_idx + k * colSize];
-        result[min_idx + k * colSize] = temp;
-      }
-    }
-  }
-
-  static void convertDateToJD2000(const T1 time, T2 julianDate) {
-    // time = { Y, M, D, h, m,s}
-    // time in sec and microsec -> The Epoch (unixtime)
-    julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
-                 floor(275 * time[1] / 9) + time[2] +
-                 (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
-  }
-
-  static T1 convertUnixToJD2000(timeval time) {
-    // time = {{s},{us}}
-    T1 julianDate2000;
-    julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
-    return julianDate2000;
-  }
-
-  static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
-    // convention q = [qx,qy,qz, qw]
-    outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
-    outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
-    outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
-
-    outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
-    outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
-    outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
-
-    outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
-    outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
-    outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
-  }
-
-  static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
-                                      T2 *cartesianOutput) {
-    /* @brief:  cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
-     * 								longitude and altitude
-     * @param:  lat   				geodetic latitude [rad]
-     * 			longi 				longitude [rad]
-     * 			alt					altitude [m]
-     * 			cartesianOutput 	Cartesian Coordinates in ECEF (3x1)
-     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
-     * 			Landis Markley and John L. Crassidis*/
-    double radiusPolar = 6356752.314;
-    double radiusEqua = 6378137;
-
-    double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
-    double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
-
-    cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
-    cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
-    cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
-  }
-
-  static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
-    /* @brief:  latLongAltFromCartesian() - calculates latitude, longitude and altitude from
-     * cartesian coordinates in ECEF
-     * @param:  x   			x-value of position vector [m]
-     * 			y 				y-value of position vector [m]
-     * 			z				z-value of position vector [m]
-     * 			latitude 		geodetic latitude [rad]
-     * 			longitude		longitude [rad]
-     * 			altitude		altitude [m]
-     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
-     * 			Landis Markley and John L. Crassidis*/
-    // From World Geodetic System the Earth Radii
-    double a = 6378137.0;     // semimajor axis [m]
-    double b = 6356752.3142;  // semiminor axis [m]
-
-    // Calculation
-    double e2 = 1 - pow(b, 2) / pow(a, 2);
-    double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
-    double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
-    double p = std::abs(vector[2]) / epsilon2;
-    double s = pow(rho, 2) / (e2 * epsilon2);
-    double q = pow(p, 2) - pow(b, 2) + s;
-    double u = p / sqrt(q);
-    double v = pow(b, 2) * pow(u, 2) / q;
-    double P = 27 * v * s / q;
-    double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
-    double t = (1 + Q + 1 / Q) / 6;
-    double c = sqrt(pow(u, 2) - 1 + 2 * t);
-    double w = (c - u) / 2;
-    double d =
-        sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
-    double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
-    latitude = asin((epsilon2 + 1) * d / N);
-    altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
-    longitude = atan2(vector[1], vector[0]);
-  }
-
-  static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
-    /* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
-     * @param:  time   			Current time
-     * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
-     * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
-     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
-     * 			Landis Markley and John L. Crassidis*/
-    double JD2000Floor = 0;
-    double JD2000 = convertUnixToJD2000(time);
-    // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
-    JD2000Floor = floor(JD2000);
-    if ((JD2000 - JD2000Floor) < 0.5) {
-      JD2000Floor -= 0.5;
-    } else {
-      JD2000Floor += 0.5;
-    }
-
-    double JC2000 = JD2000Floor / 36525;
-    double sec = (JD2000 - JD2000Floor) * 86400;
-    double gmst = 0;  // greenwich mean sidereal time
-    gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
-           0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
-    double rest = gmst / 86400;
-    double FloorRest = floor(rest);
-    double secOfDay = rest - FloorRest;
-    secOfDay *= 86400;
-    gmst = secOfDay / 240 * M_PI / 180;
-
-    outputDcmEJ[0] = cos(gmst);
-    outputDcmEJ[1] = sin(gmst);
-    outputDcmEJ[2] = 0;
-    outputDcmEJ[3] = -sin(gmst);
-    outputDcmEJ[4] = cos(gmst);
-    outputDcmEJ[5] = 0;
-    outputDcmEJ[6] = 0;
-    outputDcmEJ[7] = 0;
-    outputDcmEJ[8] = 1;
-
-    //	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
-    double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
-                              {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
-                              {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
-    double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
-    double omegaEarth = 0.000072921158553;
-    double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
-    MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
-  }
-
-  /* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
-   * 			give also the back the derivative of this matrix
-   * @param:  unixTime   		Current time in Unix format
-   * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
-   * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
-   * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
-                          Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
-   * 			Oliver Zeile
-   *
-   https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
-  static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
-    //		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
-    //(initial Offset) 		International Atomic Time (TAI)
-
-    double JD2000UTC1 = convertUnixToJD2000(unixTime);
-
-    //		Julian Date / century from TT
-    timeval terestrialTime = unixTime;
-    terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
-    double JD2000TT = convertUnixToJD2000(terestrialTime);
-    double JC2000TT = JD2000TT / 36525;
-
-    //-------------------------------------------------------------------------------------
-    //		Calculation of Transformation from earth rotation Theta
-    //-------------------------------------------------------------------------------------
-    double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    //		Earth Rotation angle
-    double era = 0;
-    era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
-    //		Greenwich Mean Sidereal Time
-    double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
-                      0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
-    double arcsecFactor = 1 * M_PI / (180 * 3600);
-    gmst2000 *= arcsecFactor;
-    gmst2000 += era;
-
-    theta[0][0] = cos(gmst2000);
-    theta[0][1] = sin(gmst2000);
-    theta[0][2] = 0;
-    theta[1][0] = -sin(gmst2000);
-    theta[1][1] = cos(gmst2000);
-    theta[1][2] = 0;
-    theta[2][0] = 0;
-    theta[2][1] = 0;
-    theta[2][2] = 1;
-
-    //-------------------------------------------------------------------------------------
-    //		Calculation of Transformation from earth Precession P
-    //-------------------------------------------------------------------------------------
-    double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-
-    double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
-    double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
-    double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
-
-    zeta *= arcsecFactor;
-    theta2 *= arcsecFactor;
-    ze *= arcsecFactor;
-
-    precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
-    precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
-    precession[2][0] = sin(theta2) * cos(zeta);
-    precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
-    precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
-    precession[2][1] = -sin(theta2) * sin(zeta);
-    precession[0][2] = -cos(ze) * sin(theta2);
-    precession[1][2] = -sin(ze) * sin(theta2);
-    precession[2][2] = cos(theta2);
-
-    //-------------------------------------------------------------------------------------
-    //		Calculation of Transformation from earth Nutation N
-    //-------------------------------------------------------------------------------------
-    double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    //      lunar asc node
-    double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
-                7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
-    Om *= arcsecFactor;
-    //      delta psi approx
-    double dp = -17.2 * arcsecFactor * sin(Om);
-
-    //      delta eps approx
-    double de = 9.203 * arcsecFactor * cos(Om);
-
-    //        % true obliquity of the ecliptic eps p.71 (simplified)
-    double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
-
-    nutation[0][0] = cos(dp);
-    nutation[1][0] = cos(e + de) * sin(dp);
-    nutation[2][0] = sin(e + de) * sin(dp);
-    nutation[0][1] = -cos(e) * sin(dp);
-    nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
-    nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
-    nutation[0][2] = -sin(e) * sin(dp);
-    nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
-    nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
-
-    //-------------------------------------------------------------------------------------
-    //		Calculation of Derivative of rotation matrix from earth
-    //-------------------------------------------------------------------------------------
-    double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
-    double omegaEarth = 0.000072921158553;
-    MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
-    MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
-
-    //-------------------------------------------------------------------------------------
-    //		Calculation of transformation matrix and Derivative of transformation matrix
-    //-------------------------------------------------------------------------------------
-    double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
-    MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
-
-    MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
-  }
-  static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
-    int i, j;
-    double determinant = 0;
-    double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
-                        {matrix[3], matrix[4], matrix[5]},
-                        {matrix[6], matrix[7], matrix[8]}};
-
-    for (i = 0; i < 3; i++) {
-      determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
-                                                mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
-    }
-    //		cout<<"\n\ndeterminant: "<<determinant;
-    //		cout<<"\n\nInverse of matrix is: \n";
-    for (i = 0; i < 3; i++) {
-      for (j = 0; j < 3; j++) {
-        output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
-                             (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
-                            determinant;
-      }
-    }
-  }
-
-  static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
-    /* do not use this. takes 300ms */
-    float det = 0;
-    T1 matrix[size][size], submatrix[size - 1][size - 1];
-    for (uint8_t row = 0; row < size; row++) {
-      for (uint8_t col = 0; col < size; col++) {
-        matrix[row][col] = inputMatrix[row * size + col];
-      }
-    }
-    if (size == 2)
-      return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
-    else {
-      for (uint8_t col = 0; col < size; col++) {
-        int subRow = 0;
-        for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
-          int subCol = 0;
-          for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
-            if (colIndex == col) continue;
-            submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
-            subCol++;
-          }
-          subRow++;
-        }
-        det += (pow(-1, col) * matrix[0][col] *
-                MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
-      }
-    }
-    return det;
-  }
-
-  static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
-    // Stopwatch stopwatch;
-    T1 matrix[size][size], identity[size][size];
-    // reformat array to matrix
-    for (uint8_t row = 0; row < size; row++) {
-      for (uint8_t col = 0; col < size; col++) {
-        matrix[row][col] = inputMatrix[row * size + col];
-      }
-    }
-    // init identity matrix
-    std::memset(identity, 0.0, sizeof(identity));
-    for (uint8_t diag = 0; diag < size; diag++) {
-      identity[diag][diag] = 1;
-    }
-    // gauss-jordan algo
-    // sort matrix such as no diag entry shall be 0
-    for (uint8_t row = 0; row < size; row++) {
-      if (matrix[row][row] == 0.0) {
-        bool swaped = false;
-        uint8_t rowIndex = 0;
-        while ((rowIndex < size) && !swaped) {
-          if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
-            for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
-              std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
-              std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
-            }
-            swaped = true;
-          }
-          rowIndex++;
-        }
-        if (!swaped) {
-          return 1;  // matrix not invertible
-        }
-      }
-    }
-
-    for (int row = 0; row < size; row++) {
-      if (matrix[row][row] == 0.0) {
-        uint8_t rowIndex;
-        if (row == 0) {
-          rowIndex = size - 1;
-        } else {
-          rowIndex = row - 1;
-        }
-        for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
-          std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
-          std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
-        }
-        row--;
-        if (row < 0) {
-          return 1;  // Matrix is not invertible
-        }
-      }
-    }
-    // remove non diag elements in matrix (jordan)
-    for (int row = 0; row < size; row++) {
-      for (int rowIndex = 0; rowIndex < size; rowIndex++) {
-        if (row != rowIndex) {
-          double ratio = matrix[rowIndex][row] / matrix[row][row];
-          for (int colIndex = 0; colIndex < size; colIndex++) {
-            matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
-            identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
-          }
-        }
-      }
-    }
-    // normalize rows in matrix (gauss)
-    for (int row = 0; row < size; row++) {
-      for (int col = 0; col < size; col++) {
-        identity[row][col] = identity[row][col] / matrix[row][row];
-      }
-    }
-    std::memcpy(inverse, identity, sizeof(identity));
-    return 0;  // successful inversion
-  }
-
-  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
-    for (uint8_t i = 0; i < size; i++) {
-      if (not isfinite(inputVector[i])) {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
-    for (uint8_t col = 0; col < cols; col++) {
-      for (uint8_t row = 0; row < rows; row++) {
-        if (not isfinite(inputMatrix[row * cols + cols])) {
-          return false;
-        }
-      }
-    }
-    return true;
-  }
-};
-
-#endif /* ACS_MATH_MATHOPERATIONS_H_ */

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
 //! [EXPORT] : [COMMENT] Multiple RWs have failed.
 static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
 
+struct RwAvail {
+  bool rw1avail = false;
+  bool rw2avail = false;
+  bool rw3avail = false;
+  bool rw4avail = false;
+};
+
 enum SetIds : uint32_t {
   MGM_SENSOR_DATA,
   MGM_PROCESSED_DATA,

mission/genericFactory.cpp

@@ -250,20 +250,30 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
                            pus::PUS_SERVICE_2, 3, 10);
   new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID,
                            pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16);
-  new Service5EventReporting(
-      PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
-      80, 160);
+
+  auto psbParamsService5 =
+      PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5);
+  psbParamsService5.requestQueueDepth = 50;
+  psbParamsService5.maxPacketsPerCycle = 50;
+  new Service5EventReporting(psbParamsService5, 80, 160);
   new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
                                  pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60);
   new Service9TimeManagement(
       PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9));
 
-  new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
-      PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11),
-      ccsdsDistrib);
+  auto psbParamsService11 =
+      PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11);
+  psbParamsService11.requestQueueDepth = 100;
+  psbParamsService11.maxPacketsPerCycle = 100;
+  new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(psbParamsService11,
+                                                                     ccsdsDistrib);
+
   new Service15TmStorage(objects::PUS_SERVICE_15_TM_STORAGE, config::EIVE_PUS_APID, 10);
-  new Service17Test(
-      PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17));
+  auto psbParamsService17 =
+      PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17);
+  psbParamsService17.requestQueueDepth = 50;
+  psbParamsService17.maxPacketsPerCycle = 50;
+  new Service17Test(psbParamsService17);
   new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID,
                                    pus::PUS_SERVICE_20);
   new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID,

mission/sysDefs.h

@@ -55,6 +55,7 @@ static constexpr char VERSION_FILE_NAME[] = "version.txt";
 static constexpr char LEGACY_REBOOT_WATCHDOG_FILE_NAME[] = "reboot.txt";
 static constexpr char REBOOT_WATCHDOG_FILE_NAME[] = "reboot_watchdog.txt";
 static constexpr char REBOOT_COUNTER_FILE_NAME[] = "reboot_counters.txt";
+static constexpr char LEAP_SECONDS_FILE_NAME[] = "leapseconds.txt";
 static constexpr char TIME_FILE_NAME[] = "time_backup.txt";
 
 static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000;
@@ -93,6 +94,8 @@ static constexpr ActionId_t MV_HELPER = 53;
 static constexpr ActionId_t RM_HELPER = 54;
 static constexpr ActionId_t MKDIR_HELPER = 55;
 
+static constexpr ActionId_t UPDATE_LEAP_SECONDS = 60;
+
 static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CORE;
 
 static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM);

mission/system/acs/StrFdir.cpp

@@ -1,6 +1,8 @@
 #include "StrFdir.h"
 
-#include "mission/acs/defs.h"
+#include <eive/objects.h>
+#include <fsfw/events/EventManagerIF.h>
+#include <mission/acs/defs.h>
 
 StrFdir::StrFdir(object_id_t strObject)
     : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
@@ -12,3 +14,13 @@ ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
   }
   return DeviceHandlerFailureIsolation::eventReceived(event);
 }
+
+ReturnValue_t StrFdir::initialize() {
+  ReturnValue_t result = DeviceHandlerFailureIsolation::initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
+  return manager->subscribeToEvent(eventQueue->getId(),
+                                   event::getEventId(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION));
+}

mission/system/acs/StrFdir.h

@@ -6,6 +6,7 @@
 class StrFdir : public DeviceHandlerFailureIsolation {
  public:
   StrFdir(object_id_t strObject);
+  ReturnValue_t initialize() override;
   ReturnValue_t eventReceived(EventMessage* event) override;
 };