mission/controller/AcsController.cpp

@@ -249,15 +249,18 @@ void AcsController::performPointingCtrl() {
 
   double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
   switch (submode) {
-    case SUBMODE_PTG_TARGET:
-      guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat,
-                             refSatRate);
-      break;
     case SUBMODE_PTG_SUN:
-      guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+      guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
+                          targetQuat, refSatRate);
       break;
+    case SUBMODE_PTG_TARGET:
+      guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
+                                      refSatRate);
+      break;
+
     case SUBMODE_PTG_NADIR:
-      guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+      guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
+                                     refSatRate);
       break;
     case SUBMODE_PTG_INERTIAL:
       guidance.inertialQuatPtg(targetQuat, refSatRate);

mission/controller/acs/Guidance.cpp

@@ -30,9 +30,10 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
   //	memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
 }
 
-void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                             acsctrl::SusDataProcessed *susDataProcessed, timeval now,
+                                       acsctrl::SusDataProcessed *susDataProcessed,
-                             double targetQuat[4], double refSatRate[3]) {
+                                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                                       double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
@@ -102,9 +103,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   //	Calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
   double velSatE[3] = {0, 0, 0};
-  velSatE[0] = outputValues->gpsVelocity[0];  // sensorValues->gps0Velocity[0];
+  std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
-  velSatE[1] = outputValues->gpsVelocity[1];  // sensorValues->gps0Velocity[1];
-  velSatE[2] = outputValues->gpsVelocity[2];  // sensorValues->gps0Velocity[2];
   double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
   //	Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
   MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
@@ -127,7 +126,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
     double sunDirB[3] = {0, 0, 0};
 
-    if (outputValues->sunDirModelValid) {
+    if (susDataProcessed->sunIjkModel.isValid()) {
       double sunDirJ[3] = {0, 0, 0};
       std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
       MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
@@ -174,8 +173,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   }
 }
 
-void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
-                             timeval now, double targetQuat[4], double refSatRate[3]) {
+                                      acsctrl::GpsDataProcessed *gpsDataProcessed,
+                                      acsctrl::MekfData *mekfData, timeval now,
+                                      double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for target pointing
   //-------------------------------------------------------------------------------------
@@ -217,8 +218,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
   // Transform velocity into inertial frame
-  double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1],
+  double velocityE[3];
-                         outputValues->gpsVelocity[2]};
+  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
   double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
@@ -251,7 +252,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   double timeElapsed =
       now.tv_sec + now.tv_usec * pow(10, -6) -
       (timeSavedQuaternionNadir.tv_sec +
-       timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6));
+       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
   if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
     double qDiff[4] = {0, 0, 0, 0};
     VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
@@ -287,38 +288,28 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
-  quatBJ[0] = outputValues->quatMekfBJ[0];
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  quatBJ[1] = outputValues->quatMekfBJ[1];
-  quatBJ[2] = outputValues->quatMekfBJ[2];
-  quatBJ[3] = outputValues->quatMekfBJ[3];
   QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
 }
 
-void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                          timeval now, double targetQuat[4], double refSatRate[3]) {
+                          acsctrl::SusDataProcessed *susDataProcessed,
+                          acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                          double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to sun
   //-------------------------------------------------------------------------------------
   double quatBJ[4] = {0, 0, 0, 0};
   double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  quatBJ[0] = outputValues->quatMekfBJ[0];
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  quatBJ[1] = outputValues->quatMekfBJ[1];
-  quatBJ[2] = outputValues->quatMekfBJ[2];
-  quatBJ[3] = outputValues->quatMekfBJ[3];
   QuaternionOperations::toDcm(quatBJ, dcmBJ);
 
   double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
-  if (outputValues->sunDirModelValid) {
+  if (susDataProcessed->sunIjkModel.isValid()) {
-    sunDirJ[0] = outputValues->sunDirModel[0];
+    std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
-    sunDirJ[1] = outputValues->sunDirModel[1];
-    sunDirJ[2] = outputValues->sunDirModel[2];
     MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-  }
+  } else {
-
+    std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
-  else {
-    sunDirB[0] = outputValues->sunDirEst[0];
-    sunDirB[1] = outputValues->sunDirEst[1];
-    sunDirB[2] = outputValues->sunDirEst[2];
   }
 
   /*
@@ -367,8 +358,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou
   double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
   MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                   sensorValues->gpsSet.altitude.value, posSatE);
-  double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1],
+  double velocityE[3];
-                         outputValues->gpsVelocity[2]};
+  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
   double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
@@ -410,9 +401,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou
   refSatRate[2] = 0;
 }
 
-void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues,
+void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                                      ACS::OutputValues *outputValues, timeval now,
+                                      timeval now, double targetQuat[4],
-                                      double targetQuat[4],
                                       double refSatRate[3]) {  // old version of Nadir Pointing
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
@@ -440,10 +430,7 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues,
   double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double quatBJ[4] = {0, 0, 0, 0};
-  quatBJ[0] = outputValues->quatMekfBJ[0];
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  quatBJ[1] = outputValues->quatMekfBJ[1];
-  quatBJ[2] = outputValues->quatMekfBJ[2];
-  quatBJ[3] = outputValues->quatMekfBJ[3];
   QuaternionOperations::toDcm(quatBJ, dcmBJ);
   MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
 
@@ -478,8 +465,10 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues,
   refSatRate[2] = 0;
 }
 
-void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
-                            timeval now, double targetQuat[4], double refSatRate[3]) {
+                                     acsctrl::GpsDataProcessed *gpsDataProcessed,
+                                     acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                                     double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
   //-------------------------------------------------------------------------------------
@@ -512,9 +501,8 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *
   VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
   // z-Axis parallel to long side of picture resolution
-  double zAxis[3] = {0, 0, 0};
+  double zAxis[3] = {0, 0, 0}, velocityE[3];
-  double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1],
+  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
-                         outputValues->gpsVelocity[2]};
   double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
@@ -539,7 +527,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *
   double timeElapsed =
       now.tv_sec + now.tv_usec * pow(10, -6) -
       (timeSavedQuaternionNadir.tv_sec +
-       timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6));
+       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
   if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
     double qDiff[4] = {0, 0, 0, 0};
     VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
@@ -575,10 +563,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
-  quatBJ[0] = outputValues->quatMekfBJ[0];
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  quatBJ[1] = outputValues->quatMekfBJ[1];
-  quatBJ[2] = outputValues->quatMekfBJ[2];
-  quatBJ[3] = outputValues->quatMekfBJ[3];
   QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
 }
 
@@ -608,7 +593,6 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
                                {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
                                {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
 
-  double quatErrorComplete[4] = {0, 0, 0, 0};
   MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
 
   if (quatErrorComplete[3] < 0) {
@@ -619,8 +603,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
   quatError[1] = quatErrorComplete[1];
   quatError[2] = quatErrorComplete[2];
 
-  // target flag in matlab, importance, does look like it only gives
+  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  //	feedback if pointing control is under 150 arcsec ??
+  // under 150 arcsec ??
 }
 
 void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {

mission/controller/acs/Guidance.h

@@ -23,26 +23,31 @@ class Guidance {
 
   // Function to get the target quaternion and refence rotation rate from gps position and position
   // of the ground station
-  void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+  void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
-                               acsctrl::SusDataProcessed *susDataProcessed, timeval now,
+                              acsctrl::GpsDataProcessed *gpsDataProcessed,
+                              acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                              double refSatRate[3]);
+  void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                               acsctrl::SusDataProcessed *susDataProcessed,
+                               acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
                                double targetQuat[4], double refSatRate[3]);
-  void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                     acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
-                     double refSatRate[3]);
 
   // Function to get the target quaternion and refence rotation rate for sun pointing after ground
   // station
-  void sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now,
+  void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                  double targetQuat[4], double refSatRate[3]);
+                  acsctrl::SusDataProcessed *susDataProcessed,
+                  acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
+                  double refSatRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
-  void quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+  void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
+                             acsctrl::GpsDataProcessed *gpsDataProcessed,
+                             acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                             double refSatRate[3]);
+  void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                               timeval now, double targetQuat[4], double refSatRate[3]);
 
-  void quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now,
-                    double targetQuat[4], double refSatRate[3]);
-
   // Function to get the target quaternion and refence rotation rate from parameters for inertial
   // pointing
   void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);