From 42b94ab581f2b32b7339512347f5b563f8b14455 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 26 Jan 2024 10:50:11 +0100
Subject: [PATCH] we can only use the nullspace if all rws are available

---
 mission/controller/acs/control/PtgCtrl.cpp | 8 ++++++--
 mission/controller/acs/control/PtgCtrl.h   | 3 ++-
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp
index fd5b9f29..8c3a7e7b 100644
--- a/mission/controller/acs/control/PtgCtrl.cpp
+++ b/mission/controller/acs/control/PtgCtrl.cpp
@@ -39,7 +39,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
 
   double cInt = 2 * om * zeta;
-  double kInt = 2 * pow(om, 2);
+  double kInt = 2 * om * om;
 
   double qErrorLaw[3] = {0, 0, 0};
 
@@ -111,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
+                           AcsParameters::PointingLawParameters *pointingLawParameters,
                            const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                            const int32_t speedRw3, double *rwTrqNs) {
+  if (not allRwAvabilable) {
+    return;
+  }
   // concentrate RW speeds as vector and convert to double
   double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
                         static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h
index a07849ec..66402b0d 100644
--- a/mission/controller/acs/control/PtgCtrl.h
+++ b/mission/controller/acs/control/PtgCtrl.h
@@ -33,7 +33,8 @@ class PtgCtrl {
   void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
               const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
 
-  void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+  void ptgNullspace(const bool allRwAvabilable,
+                    AcsParameters::PointingLawParameters *pointingLawParameters,
                     const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                     const int32_t speedRw3, double *rwTrqNs);