added PtgCtrl code. matched inputs to actual SensorValues inputs

mission/controller/AcsController.cpp

@@ -1,14 +1,15 @@
 #include "AcsController.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
-//#include <fsfw/timemanager/Clock.h>
 
 AcsController::AcsController(object_id_t objectId)
     : ExtendedControllerBase(objectId, objects::NO_OBJECT),
       sensorProcessing(&acsParameters),
       navigation(&acsParameters),
       actuatorCmd(&acsParameters),
+      guidance(&acsParameters),
       detumble(&acsParameters),
+      ptgCtrl(&acsParameters),
       detumbleCounter{0},
       mgmData(this),
       susData(this) {}
@@ -42,7 +43,7 @@ void AcsController::performControlOperation() {
             break;
 
           case SUBMODE_PTG_GS:
-            // performPointingCtrl();
+            performPointingCtrl();
             break;
         }
       }
@@ -65,7 +66,6 @@ void AcsController::performControlOperation() {
     }
   }
 
-
   // DEBUG : REMOVE AFTER COMPLETION
   mode = MODE_ON;
   submode = SUBMODE_DETUMBLE;
@@ -81,7 +81,7 @@ void AcsController::performDetumble() {
   timeval now;
   Clock::getClock_timeval(&now);
 
-  sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters);
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
   ReturnValue_t validMekf;
   navigation.useMekf(&sensorValues, &outputValues, &validMekf);
   double magMomMtq[3] = {0, 0, 0};
@@ -108,7 +108,37 @@ void AcsController::performDetumble() {
   }
 }
 
-void AcsController::performPointingCtrl() {}
+void AcsController::performPointingCtrl() {
+  ACS::SensorValues sensorValues;
+  ACS::OutputValues outputValues;
+
+  timeval now;  // Übergabe ?
+
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  ReturnValue_t validMekf;
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
+  guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+  double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
+  guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
+  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
+  ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
+  double rwTrqNs[4] = {0, 0, 0, 0};
+  ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2),
+                       &(sensorValues.speedRw3), rwTrqNs);
+  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
+  actuatorCmd.cmdSpeedToRws(&(sensorValues.speedRw0), &(sensorValues.speedRw1),
+                            &(sensorValues.speedRw2), &(sensorValues.speedRw3), torquePtgRws,
+                            rwTrqNs, cmdSpeedRws);
+  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
+  ptgCtrl.ptgDesaturation(outputValues.magFieldEst, &outputValues.magFieldEstValid,
+                          outputValues.satRateMekf, &(sensorValues.speedRw0),
+                          &(sensorValues.speedRw1), &(sensorValues.speedRw2),
+                          &(sensorValues.speedRw3), mgtDpDes);
+  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
+}
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {

mission/controller/AcsController.h

@@ -6,9 +6,11 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 
 #include "acs/ActuatorCmd.h"
+#include "acs/Guidance.h"
 #include "acs/Navigation.h"
 #include "acs/SensorProcessing.h"
 #include "acs/control/Detumble.h"
+#include "acs/control/PtgCtrl.h"
 #include "controllerdefinitions/AcsCtrlDefinitions.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
@@ -37,8 +39,11 @@ class AcsController : public ExtendedControllerBase {
   SensorProcessing sensorProcessing;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
+  Guidance guidance;
 
   Detumble detumble;
+  PtgCtrl ptgCtrl;
+
   uint8_t detumbleCounter;
 
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };

mission/controller/acs/Navigation.cpp

@@ -6,45 +6,46 @@
  */
 
 #include "Navigation.h"
-#include "util/MathOperations.h"
-#include "util/CholeskyDecomposition.h"
-#include <math.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
+
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
 
+#include "util/CholeskyDecomposition.h"
+#include "util/MathOperations.h"
 
-Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){
-	acsParameters = *acsParameters_;
+Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
+  acsParameters = *acsParameters_;
 }
 
-Navigation::~Navigation(){
+Navigation::~Navigation() {}
 
+void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+                         ReturnValue_t *mekfValid) {
+  double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
+  bool quatJBValid =
+      (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() &&
+       sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid());
+
+  if (kalmanInit) {
+    *mekfValid = multiplicativeKalmanFilter.mekfEst(
+        quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid,
+        outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst,
+        &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
+        outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
+        outputValues->satRateMekf);  // VALIDS FOR QUAT AND RATE ??
+  }
+  else {
+    multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
+                                    outputValues->sunDirEst, &outputValues->sunDirEstValid,
+                                    outputValues->sunDirModel, &outputValues->sunDirModelValid,
+                                    outputValues->magFieldModel, &outputValues->magFieldModelValid);
+    kalmanInit = true;
+    *mekfValid = 0;
+
+    //		Maybe we need feedback from kalmanfilter to identify if there was a problem with the init
+    //of kalman filter 		where does this class know from that kalman filter was not initialized ?
+  }
 }
-
-void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){
-
-	if (kalmanInit) {
-		*mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid,
-				outputValues->satRateEst, &outputValues->satRateEstValid,
-				outputValues->magFieldEst, &outputValues->magFieldEstValid,
-				outputValues->sunDirEst, &outputValues->sunDirEstValid,
-				outputValues->sunDirModel, &outputValues->sunDirModelValid,
-				outputValues->magFieldModel, &outputValues->magFieldModelValid,
-				outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
-
-	}
-	else {
-		multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
-				outputValues->sunDirEst, &outputValues->sunDirEstValid,
-				outputValues->sunDirModel, &outputValues->sunDirModelValid,
-				outputValues->magFieldModel, &outputValues->magFieldModelValid);
-		kalmanInit = true;
-		*mekfValid = 0;
-
-//		Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter
-//		where does this class know from that kalman filter was not initialized ?
-	}
-}
-
-