RW cmd fix and cleanup

mission/controller/AcsController.cpp

@@ -489,10 +489,9 @@ void AcsController::performPointingCtrl() {
 
   actuatorCmd.cmdSpeedToRws(
       sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
-      cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
-      acsParameters.rwHandlingParameters.maxRwSpeed,
-      acsParameters.rwHandlingParameters.inertiaWheel);
+      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+      acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
+      acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
   if (enableAntiStiction) {
     ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
   }

mission/controller/acs/ActuatorCmd.cpp

@@ -5,8 +5,6 @@
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 
-#include <cmath>
-
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
 
@@ -25,24 +23,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
   }
 }
 
-void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
-                                int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
-                                double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
-  using namespace Math;
-
-  // Calculating the commanded speed in RPM for every reaction wheel
+void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
+                                const int32_t speedRw2, const int32_t speedRw3,
+                                const double sampleTime, const double inertiaWheel,
+                                const int32_t maxRwSpeed, const double *rwTorque,
+                                int32_t *rwCmdSpeed) {
+  // concentrate RW speed values (in 0.1 [RPM]) in vector
   int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
+
+  // calculate required RW speed as sum of current RW speed and RW speed delta
+  // delta w_rw = delta t / I_RW * torque_RW [rad/s]
   double deltaSpeed[4] = {0, 0, 0, 0};
-  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
-  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
-  double factor = sampleTime / inertiaWheel * radToRpm;
-  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
+  const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
   VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
+
+  // convert double to int32
+  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
   for (int i = 0; i < 4; i++) {
     deltaSpeedInt[i] = std::round(deltaSpeed[i]);
   }
+
+  // sum of current RW speed and RW speed delta
   VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
-  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
+
+  // crop values which would exceed the maximum possible RPM
   for (uint8_t i = 0; i < 4; i++) {
     if (rwCmdSpeed[i] > maxRwSpeed) {
       rwCmdSpeed[i] = maxRwSpeed;
@@ -54,11 +58,11 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
 
 void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
                               const double *inverseAlignment, double maxDipol) {
-  // Convert to actuator frame
+  // convert to actuator frame
   double dipolMomentActuatorDouble[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
                                      1);
-  // Scaling along largest element if dipol exceeds maximum
+  // scaling along largest element if dipole exceeds maximum
   uint8_t maxIdx = 0;
   VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
   double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);

mission/controller/acs/ActuatorCmd.h

@@ -1,9 +1,8 @@
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_
 
-#include "MultiplicativeKalmanFilter.h"
-#include "SensorProcessing.h"
-#include "SensorValues.h"
+#include <cmath>
+
 
 class ActuatorCmd {
  public:
@@ -19,17 +18,16 @@ class ActuatorCmd {
   void scalingTorqueRws(double *rwTrq, double maxTorque);
 
   /*
-   * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
-   * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
-   * as Input to the RWs
-   * @param:	rwTrqIn 			given torque from pointing controller
-   * 			rwTrqNS				Nullspace torque
+   * @brief:	cmdSpeedToRws()		Calculates the RPM for the reaction wheel configuration,
+   * given the required torque calculated by the controller. Will also scale down the RPM of the
+   * wheels if they exceed the maximum possible RPM
+   * @param:	rwTrq 			    given torque from pointing controller
    * 			rwCmdSpeed			output revolutions per minute for every
    * reaction wheel
    */
-  void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
-                     const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
-                     int32_t maxRwSpeed, double inertiaWheel);
+  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                     const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
+                     const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
 
   /*
    * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
@@ -42,6 +40,7 @@ class ActuatorCmd {
 
  protected:
  private:
+  static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
 };
 
 #endif /* ACTUATORCMD_H_ */