From c66799b24f877c1c59186ede0bd6de9f473d66a0 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 11 Apr 2023 10:55:48 +0200
Subject: [PATCH] removed unit quats as they are confusing

---
 mission/controller/AcsController.cpp                | 13 +++++++------
 mission/controller/AcsController.h                  |  7 ++++---
 .../controller/acs/MultiplicativeKalmanFilter.cpp   |  6 ++----
 mission/controller/acs/MultiplicativeKalmanFilter.h |  3 +++
 4 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 517ac8f3..ab24f647 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -576,13 +576,13 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
 void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
-    std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
     ctrlValData.tgtQuat.setValid(false);
-    std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
     ctrlValData.errQuat.setValid(false);
     ctrlValData.errAng.value = errAng;
     ctrlValData.errAng.setValid(true);
-    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
     ctrlValData.safeStrat.value = safeModeStrat;
     ctrlValData.safeStrat.setValid(true);
@@ -606,10 +606,10 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
 void AcsController::disableCtrlValData() {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
-    std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
-    std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
+    std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
     ctrlValData.errAng.value = 0;
-    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.setValidity(false, true);
   }
 }
@@ -691,6 +691,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
   poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
   // Ctrl Values
+  localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 10f32720..6ec26c57 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -40,9 +40,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void performPointingCtrl();
 
  private:
-  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
-  static constexpr double ZERO_VEC[3] = {0, 0, 0};
-  static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
+  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+  static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
   static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
 
   bool enableHkSets = false;
@@ -214,6 +214,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   // Ctrl Values
   acsctrl::CtrlValData ctrlValData;
+  PoolEntry<uint8_t> safeStrat = PoolEntry<uint8_t>();
   PoolEntry<double> tgtQuat = PoolEntry<double>(4);
   PoolEntry<double> errQuat = PoolEntry<double>(4);
   PoolEntry<double> errAng = PoolEntry<double>();
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 88568573..0f297b21 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1105,11 +1105,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
   {
     PoolReadGuard pg(mekfData);
     if (pg.getReadResult() == returnvalue::OK) {
-      double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-      double zeroVec[3] = {0.0, 0.0, 0.0};
-      std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+      std::memcpy(mekfData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double));
       mekfData->quatMekf.setValid(false);
-      std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+      std::memcpy(mekfData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double));
       mekfData->satRotRateMekf.setValid(false);
       mekfData->mekfStatus = mekfStatus;
       mekfData->setValidity(true, false);
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index fe749890..5a878c4b 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -80,6 +80,9 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
 
  private:
+  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+
   /*Parameters*/
   double quaternion_STR_SB[4];