fixed rw torque scaling

CHANGELOG.md

@@ -42,6 +42,7 @@ will consitute of a breaking change warranting a new major release:
   the values of the last step, which would result in undefined behaviour.
 - Solved naming collision between file used for solar array deployment and confirmation for
   ACS for solar array deployment.
+- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded.
 
 ## Changed
 

mission/controller/AcsController.cpp

@@ -291,13 +291,13 @@ void AcsController::performPointingCtrl() {
   } else {
     multipleRwUnavailableCounter = 0;
   }
-  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
-  double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
-  double mgtDpDes[3] = {0, 0, 0};
 
   // Variables required for guidance
   double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
          errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
+  // Variables required for setting actuators
+  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
+         mgtDpDes[3] = {0, 0, 0};
   switch (submode) {
     case acs::PTG_IDLE:
       guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
@@ -310,8 +310,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
-                                   acsParameters.rwHandlingParameters.maxTrq);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -335,8 +334,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
-                                   acsParameters.rwHandlingParameters.maxTrq);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -357,8 +355,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
-                                   acsParameters.rwHandlingParameters.maxTrq);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -382,8 +379,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
-                                   acsParameters.rwHandlingParameters.maxTrq);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -406,8 +402,7 @@ void AcsController::performPointingCtrl() {
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
-      actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled,
-                                   acsParameters.rwHandlingParameters.maxTrq);
+      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@@ -416,22 +411,22 @@ void AcsController::performPointingCtrl() {
       enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
   }
-  if (enableAntiStiction) {
-    ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
-  }
 
   actuatorCmd.cmdSpeedToRws(
       sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled,
+      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
       cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
       acsParameters.rwHandlingParameters.maxRwSpeed,
       acsParameters.rwHandlingParameters.inertiaWheel);
+  if (enableAntiStiction) {
+    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
+  }
   actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
                           *acsParameters.magnetorquerParameter.inverseAlignment,
                           acsParameters.magnetorquerParameter.dipolMax);
 
   updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
-  updateActuatorCmdData(torqueRwsScaled, cmdSpeedRws, cmdDipolMtqs);
+  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
   //                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],

mission/controller/acs/ActuatorCmd.cpp

@@ -14,17 +14,14 @@ ActuatorCmd::ActuatorCmd() {}
 
 ActuatorCmd::~ActuatorCmd() {}
 
-void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) {
-  double maxValue = 0;
-  for (int i = 0; i < 4; i++) {
-    if (abs(rwTrq[i]) > maxValue) {
-      maxValue = abs(rwTrq[i]);
-    }
-  }
+void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
+  uint8_t maxIdx = 0;
+  VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
+  double maxValue = rwTrq[maxIdx];
 
   if (maxValue > maxTorque) {
     double scalingFactor = maxTorque / maxValue;
-    VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
+    VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
   }
 }
 

mission/controller/acs/ActuatorCmd.h

@@ -13,10 +13,10 @@ class ActuatorCmd {
   /*
    * @brief:	scalingTorqueRws()	scales the torque via maximum part in case this part is
    * higher then the maximum torque
-   * @param:	rwTrq	 			given torque for reaction wheels
-   * 			rwTrqScaled			possible scaled torque
+   * @param:	rwTrq	 			given torque for reaction wheels which will be
+   * scaled if needed to be
    */
-  void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque);
+  void scalingTorqueRws(double *rwTrq, double maxTorque);
 
   /*
    * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction