From fd405e61a4ee9d762e382fee709d64db17e73176 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 13:53:46 +0100
Subject: [PATCH] fixed scaling of mtq dipole commands

---
 mission/controller/acs/ActuatorCmd.cpp | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index cbe6b504..37212113 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -55,14 +55,11 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentAct
   MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
                                      1);
   // Scaling along largest element if dipol exceeds maximum
-  double maxValue = 0;
-  for (int i = 0; i < 3; i++) {
-    if (abs(dipolMomentActuator[i]) > maxDipol) {
-      maxValue = abs(dipolMomentActuator[i]);
-    }
-  }
-  if (maxValue > maxDipol) {
-    double scalingFactor = maxDipol / maxValue;
+  uint8_t maxIdx = 0;
+  VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
+  double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
+  if (maxAbsValue > maxDipol) {
+    double scalingFactor = maxDipol / maxAbsValue;
     VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
                                         dipolMomentActuatorDouble, 3);
   }