mission/controller/AcsController.cpp

@@ -79,6 +79,25 @@ void AcsController::performControlOperation() {
 #if OBSW_THREAD_TRACING == 1
   trace::threadTrace(opCounter, "ACS & TCS PST");
 #endif
+  {
+    PoolReadGuard pg(&mgmDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copyMgmData();
+    }
+  }
+  {
+    PoolReadGuard pg(&susDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copySusData();
+    }
+  }
+  {
+    PoolReadGuard pg(&gyrDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copyGyrData();
+    }
+  }
+
   switch (internalState) {
     case InternalState::STARTUP: {
       initialCountdown.resetTimer();
@@ -114,25 +133,6 @@ void AcsController::performControlOperation() {
     default:
       break;
   }
-
-  {
-    PoolReadGuard pg(&mgmDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copyMgmData();
-    }
-  }
-  {
-    PoolReadGuard pg(&susDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copySusData();
-    }
-  }
-  {
-    PoolReadGuard pg(&gyrDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copyGyrData();
-    }
-  }
 }
 
 void AcsController::performSafe() {
@@ -415,7 +415,7 @@ void AcsController::performPointingCtrl() {
                             sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
   actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
 
-  updateCtrlValData(targetQuat, errorQuat, errorAngle);
+  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
   updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
@@ -477,16 +477,20 @@ void AcsController::updateCtrlValData(double errAng) {
     ctrlValData.errQuat.setValid(false);
     ctrlValData.errAng.value = errAng;
     ctrlValData.errAng.setValid(true);
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
+    ctrlValData.tgtRotRate.setValid(false);
     ctrlValData.setValidity(true, false);
   }
 }
 
-void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) {
+void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
+                                      const double *tgtRotRate) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
     std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
     ctrlValData.errAng.value = errAng;
+    std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
     ctrlValData.setValidity(true, true);
   }
 }
@@ -497,6 +501,7 @@ void AcsController::disableCtrlValData() {
     std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
     std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
     ctrlValData.errAng.value = 0;
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
     ctrlValData.setValidity(false, true);
   }
 }

mission/controller/AcsController.h

@@ -41,6 +41,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
  private:
   static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
+  static constexpr double ZERO_VEC[3] = {0, 0, 0};
   static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
   static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
 
@@ -102,7 +103,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
                              const int16_t* mtqTargetDipole);
   void updateCtrlValData(double errAng);
-  void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng);
+  void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
+                         const double* tgtRotRate);
   void disableCtrlValData();
 
   /* ACS Sensor Values */

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -6,6 +6,7 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <stdio.h>
 #include <string.h>
+
 #include <cmath>
 
 #include "util/CholeskyDecomposition.h"