added 3 axes stabilized target mode for GS contact. renamed tgt coordinates acordingly

mission/controller/acs/AcsParameters.cpp

@@ -412,16 +412,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xC):  // GroundStationParameters
+    case (0xC):  // PtgTargetParameters
       switch (parameterId & 0xFF) {
         case 0x0:
-          parameterWrapper->set(groundStationParameters.latitudeGs);
+          parameterWrapper->set(ptgTargetParameters.latitudeTgt);
           break;
         case 0x1:
-          parameterWrapper->set(groundStationParameters.longitudeGs);
+          parameterWrapper->set(ptgTargetParameters.longitudeTgt);
           break;
         case 0x2:
-          parameterWrapper->set(groundStationParameters.altitudeGs);
+          parameterWrapper->set(ptgTargetParameters.altitudeTgt);
           break;
         default:
           return INVALID_IDENTIFIER_ID;

mission/controller/acs/AcsParameters.h

@@ -854,11 +854,11 @@ class AcsParameters : public HasParametersIF {
     double timeDiffVelocityMax = 30;  //[s]
   } gpsParameters;
 
-  struct GroundStationParameters {
+  struct ptgTargetParameters {                    // Default is Stuttgart GS
-    double latitudeGs = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
-    double longitudeGs = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
-    double altitudeGs = 500;                     // [m] Altitude
+    double altitudeTgt = 500;                     // [m] Altitude
-  } groundStationParameters;                     // Stuttgart
+  } ptgTargetParameters;
 
   struct SunModelParameters {
     float domega = 36000.771;

mission/controller/acs/Guidance.cpp

@@ -39,12 +39,11 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   //-------------------------------------------------------------------------------------
   //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
   // fixed/centered frame)
-  double groundStationCart[3] = {0, 0, 0};
+  double targetCart[3] = {0, 0, 0};
 
-  MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
+  MathOperations<double>::cartesianFromLatLongAlt(
-                                                  acsParameters.groundStationParameters.longitudeGs,
+      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
-                                                  acsParameters.groundStationParameters.altitudeGs,
+      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
-                                                  groundStationCart);
 
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
@@ -55,7 +54,7 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
 
   //	Target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
 
   //	Transformation between ECEF and IJK frame
   double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -173,6 +172,48 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   }
 }
 
+void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //-------------------------------------------------------------------------------------
+  double timeElapsed =
+      now.tv_sec + now.tv_usec * pow(10, -6) -
+      (timeSavedQuaternionNadir.tv_sec +
+       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);
+    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
+
+    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
+           qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
+    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
+    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
+    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
+    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
+    VectorOperations<double>::add(sum1, sum2, sum, 3);
+    VectorOperations<double>::subtract(sum, sum3, sum, 3);
+    double omegaRefNew[3] = {0, 0, 0};
+    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
+
+    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
+    VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
+    omegaRefSavedNadir[0] = omegaRefNew[0];
+    omegaRefSavedNadir[1] = omegaRefNew[1];
+    omegaRefSavedNadir[2] = omegaRefNew[2];
+  } else {
+    refSatRate[0] = 0;
+    refSatRate[1] = 0;
+    refSatRate[2] = 0;
+  }
+
+  timeSavedQuaternionNadir = now;
+  savedQuaternionNadir[0] = quatInertialTarget[0];
+  savedQuaternionNadir[1] = quatInertialTarget[1];
+  savedQuaternionNadir[2] = quatInertialTarget[2];
+  savedQuaternionNadir[3] = quatInertialTarget[3];
+}
+
 void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
                                       acsctrl::GpsDataProcessed *gpsDataProcessed,
                                       acsctrl::MekfData *mekfData, timeval now,
@@ -182,12 +223,11 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   //-------------------------------------------------------------------------------------
   //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
   // fixed/centered frame)
-  double groundStationCart[3] = {0, 0, 0};
+  double targetCart[3] = {0, 0, 0};
 
-  MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
+  MathOperations<double>::cartesianFromLatLongAlt(
-                                                  acsParameters.groundStationParameters.longitudeGs,
+      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
-                                                  acsParameters.groundStationParameters.altitudeGs,
+      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
-                                                  groundStationCart);
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
   double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
@@ -195,7 +235,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                   sensorValues->gpsSet.altitude.value, posSatE);
   double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
 
   //	Transformation between ECEF and IJK frame
   double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -246,45 +286,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  //-------------------------------------------------------------------------------------
+  refRotationRate(now, quatInertialTarget, refSatRate);
-  //	Calculation of reference rotation rate
-  //-------------------------------------------------------------------------------------
-  double timeElapsed =
-      now.tv_sec + now.tv_usec * pow(10, -6) -
-      (timeSavedQuaternionNadir.tv_sec +
-       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);
-    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
-
-    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
-           qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
-    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
-    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
-    VectorOperations<double>::add(sum1, sum2, sum, 3);
-    VectorOperations<double>::subtract(sum, sum3, sum, 3);
-    double omegaRefNew[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
-
-    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
-    omegaRefSavedNadir[0] = omegaRefNew[0];
-    omegaRefSavedNadir[1] = omegaRefNew[1];
-    omegaRefSavedNadir[2] = omegaRefNew[2];
-  } else {
-    refSatRate[0] = 0;
-    refSatRate[1] = 0;
-    refSatRate[2] = 0;
-  }
-
-  timeSavedQuaternionNadir = now;
-  savedQuaternionNadir[0] = quatInertialTarget[0];
-  savedQuaternionNadir[1] = quatInertialTarget[1];
-  savedQuaternionNadir[2] = quatInertialTarget[2];
-  savedQuaternionNadir[3] = quatInertialTarget[3];
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -295,7 +297,75 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
 void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                                acsctrl::SusDataProcessed *susDataProcessed,
                                acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                               double targetQuat[4], double refSatRate[3]) {}
+                               double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for ground station pointing
+  //-------------------------------------------------------------------------------------
+  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
+  // fixed/centered frame)
+  double groundStationCart[3] = {0, 0, 0};
+
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart);
+  //	Position of the satellite in the earth/fixed frame via GPS
+  double posSatE[3] = {0, 0, 0};
+  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
+  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
+  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
+                                                  sensorValues->gpsSet.altitude.value, posSatE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  //	Target Direction and position vector in the inertial frame
+  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
+
+  // negative x-Axis aligned with target (Camera/E-band transmitter position)
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // get Sun Vector Model in ECI as helper vector (element of x-z plane)
+  double sunJ[3];
+  std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
+  VectorOperations<double>::normalize(sunJ, sunJ, 3);
+
+  // calculate y-axis
+  double yAxis[3];
+  VectorOperations<double>::cross(sunJ, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  // calculate z-axis
+  double zAxis[3];
+  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+  // Complete transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  double quatInertialTarget[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
+
+  refRotationRate(now, quatInertialTarget, refSatRate);
+
+  // Transform in system relative to satellite frame
+  double quatBJ[4] = {0, 0, 0, 0};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
+}
 
 void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                           acsctrl::SusDataProcessed *susDataProcessed,
@@ -317,32 +387,6 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
     std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
   }
 
-  /*
-  // ---------------------------------------------------------------------------
-  // Old version of two vector quaternion (only one axis to align)
-  // ---------------------------------------------------------------------------
-  double sunRef[3] = {0, 0, 0};
-  sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0];
-  sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1];
-  sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2];
-
-  double sunCross[3] = {0, 0, 0};
-  VectorOperations<double>::cross(sunDirB, sunRef, sunCross);
-  double normSunDir = VectorOperations<double>::norm(sunDirB, 3);
-  double normSunRef = VectorOperations<double>::norm(sunRef, 3);
-  double dotSun = VectorOperations<double>::dot(sunDirB, sunRef);
-
-  targetQuat[0] = sunCross[0];
-  targetQuat[1] = sunCross[1];
-  targetQuat[2] = sunCross[2];
-  targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun);
-
-  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
-  */
-
-  //----------------------------------------------------------------------------
-  // New version
-  //----------------------------------------------------------------------------
   //	Transformation between ECEF and IJK frame
   double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -526,45 +570,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  //-------------------------------------------------------------------------------------
+  refRotationRate(now, quatInertialTarget, refSatRate);
-  //	Calculation of reference rotation rate
-  //-------------------------------------------------------------------------------------
-  double timeElapsed =
-      now.tv_sec + now.tv_usec * pow(10, -6) -
-      (timeSavedQuaternionNadir.tv_sec +
-       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);
-    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
-
-    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
-           qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
-    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
-    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
-    VectorOperations<double>::add(sum1, sum2, sum, 3);
-    VectorOperations<double>::subtract(sum, sum3, sum, 3);
-    double omegaRefNew[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
-
-    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
-    omegaRefSavedNadir[0] = omegaRefNew[0];
-    omegaRefSavedNadir[1] = omegaRefNew[1];
-    omegaRefSavedNadir[2] = omegaRefNew[2];
-  } else {
-    refSatRate[0] = 0;
-    refSatRate[1] = 0;
-    refSatRate[2] = 0;
-  }
-
-  timeSavedQuaternionNadir = now;
-  savedQuaternionNadir[0] = quatInertialTarget[0];
-  savedQuaternionNadir[1] = quatInertialTarget[1];
-  savedQuaternionNadir[2] = quatInertialTarget[2];
-  savedQuaternionNadir[3] = quatInertialTarget[3];
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};

mission/controller/acs/Guidance.h

@@ -21,8 +21,8 @@ class Guidance {
 
   void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
 
-  // Function to get the target quaternion and refence rotation rate from gps position and position
+  // Function to get the target quaternion and refence rotation rate from gps position and
-  // of the ground station
+  // position of the ground station
   void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
                               acsctrl::GpsDataProcessed *gpsDataProcessed,
                               acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
@@ -61,6 +61,8 @@ class Guidance {
   void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
                   double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
 
+  void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate);
+
   //	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
   // reation wheel 	maybe can be done in "commanding.h"
   void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);