From 0d32bc0c0afff1d13e2141e7a5e90f8f5b18338f Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 17 Feb 2023 15:15:47 +0100
Subject: [PATCH] added target rotation rate to ctrlValData set

---
 mission/controller/AcsController.cpp                      | 8 +++++---
 mission/controller/AcsController.h                        | 1 +
 .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 4 +++-
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 86f67bfd..73730649 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -245,7 +245,8 @@ void AcsController::performPointingCtrl() {
   double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
   double mgtDpDes[3] = {0, 0, 0};
 
-  double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0};
+  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
+         errorAngle = 0;
   switch (submode) {
     case acs::PTG_IDLE:
       guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@@ -278,8 +279,8 @@ void AcsController::performPointingCtrl() {
       guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                       gpsDataProcessed.gpsVelocity.value, targetQuat,
                                       targetSatRotRate);
-
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+      guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle);
+      guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
                           deltaRate);
       ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
                      *rwPseudoInv, torquePtgRws);
@@ -536,6 +537,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
+  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
   poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
   // Actuator CMD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index e74681d4..ba76c7da 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -180,6 +180,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> tgtQuat = PoolEntry<double>(4);
   PoolEntry<double> errQuat = PoolEntry<double>(4);
   PoolEntry<double> errAng = PoolEntry<double>();
+  PoolEntry<double> tgtRotRate = PoolEntry<double>(4);
 
   // Actuator CMD
   acsctrl::ActuatorCmdData actuatorCmdData;
diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
index e3908bd0..71158868 100644
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
@@ -95,6 +95,7 @@ enum PoolIds : lp_id_t {
   TGT_QUAT,
   ERROR_QUAT,
   ERROR_ANG,
+  TGT_ROT_RATE,
   // Actuator Cmd
   RW_TARGET_TORQUE,
   RW_TARGET_SPEED,
@@ -109,7 +110,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
 static constexpr uint8_t MEKF_SET_ENTRIES = 2;
-static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
+static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
 
 /**
@@ -249,6 +250,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
   lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
   lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
   lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
+  lp_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
 
  private:
 };