mission/controller/AcsController.cpp

@@ -269,30 +269,56 @@ void AcsController::performControlOperation() {
                        &mekfData, &validMekf);
 
     double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
+    double quatRef[4] =  {0, 0, 0, 0};
+    uint8_t enableAntiStiction = true;
     switch (submode) {
       case SUBMODE_IDLE:
         guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
                             targetQuat, refSatRate);
+        for ( uint8_t i = 0; i < 4; i++ ) {
+        	quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
+        }
+        enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+
         break;
       case SUBMODE_PTG_TARGET:
         guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
                                         targetQuat, refSatRate);
+        for ( uint8_t i = 0; i < 4; i++ ) {
+        	quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
+        }
+        enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+
         break;
       case SUBMODE_PTG_TARGET_GS:
         guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
                                  now, targetQuat, refSatRate);
+
+        for ( uint8_t i = 0; i < 4; i++ ) {
+        	quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
+        }
+        enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
         break;
       case SUBMODE_PTG_NADIR:
         guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
                                        refSatRate);
+        for ( uint8_t i = 0; i < 4; i++ ) {
+        	quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i];
+        }
+        enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
         break;
       case SUBMODE_PTG_INERTIAL:
         guidance.inertialQuatPtg(targetQuat, refSatRate);
+
+        for ( uint8_t i = 0; i < 4; i++ ) {
+        	quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i];
+        }
+        enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
         break;
     }
     double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
            deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
-    guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
+    guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, 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;
@@ -305,7 +331,7 @@ void AcsController::performControlOperation() {
     VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
     actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
 
-    if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
+    if ( enableAntiStiction ) {
       bool rwAvailable[4] = {true, true, true, true};  // WHICH INPUT SENSOR SET?
       int32_t rwSpeed[4] = {
           (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),

mission/controller/acs/AcsParameters.cpp

@@ -372,21 +372,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0xE:
           parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
-        case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.omegaEarth);
-          break;
-        case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.nadirRefDirection);
-          break;
-        case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial);
-          break;
-        case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial);
-          break;
-        case 0x13:
-          parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax);
-          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -815,15 +815,7 @@ class AcsParameters : public HasParametersIF {
 
   } safeModeControllerParameters;
 
-  // ToDo: mutiple structs for different pointing mode controllers?
+  struct PointingLawParameters {
-  struct PointingModeControllerParameters {
-    double refDirection[3] = {-1, 0, 0};  // Antenna
-    double refRotRate[3] = {0, 0, 0};
-    double quatRef[4] = {0, 0, 0, 1};
-    uint8_t avoidBlindStr = true;
-    double blindAvoidStart = 1.5;
-    double blindAvoidStop = 2.5;
-    double blindRotRate = 1 * M_PI / 180;
 
 	double zeta = 0.3;
 	double om = 0.3;
@@ -836,14 +828,38 @@ class AcsParameters : public HasParametersIF {
 	uint8_t desatOn = true;
 	uint8_t enableAntiStiction = true;
 
-    double omegaEarth = 0.000072921158553;
+  } pointingLawParameters;
 
-    double nadirRefDirection[3] = {-1, 0, 0};  // Camera
+  struct TargetModeControllerParameters : PointingLawParameters {
-    double tgtQuatInertial[4] = {0, 0, 0, 1};
+	double refDirection[3] = {-1, 0, 0};  // Antenna
-    double tgtRotRateInertial[3] = {0, 0, 0};
+	double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
-    int8_t nadirTimeElapsedMax = 10;
+	// give this as an input- currently en calculation is done
-  } pointingModeControllerParameters, inertialModeControllerParameters,
+	double quatRef[4] = {0, 0, 0, 1};
-      nadirModeControllerParameters, targetModeControllerParameters;
+	int8_t timeElapsedMax = 10; // rot rate calculations
+
+	// Default is Stuttgart GS
+	double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+	double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+	double altitudeTgt = 500; // [m]
+
+	// For one-axis control:
+	uint8_t avoidBlindStr = true;
+	double blindAvoidStart = 1.5;
+	double blindAvoidStop = 2.5;
+	double blindRotRate = 1 * M_PI / 180;
+  } targetModeControllerParameters;
+
+  struct NadirModeControllerParameters : PointingLawParameters {
+	double refDirection[3] = {-1, 0, 0};  // Antenna
+	double quatRef[4] = {0, 0, 0, 1};
+    int8_t timeElapsedMax = 10; // rot rate calculations
+  } nadirModeControllerParameters;
+
+  struct InertialModeControllerParameters : PointingLawParameters {
+	double tgtQuat[4] = {0, 0, 0, 1};
+	double refRotRate[3] = {0, 0, 0};
+	double quatRef[4] = {0, 0, 0, 1};
+  } inertialModeControllerParameters;
 
   struct StrParameters {
     double exclusionAngle = 20 * M_PI / 180;

mission/controller/acs/Guidance.cpp

@@ -42,8 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   double targetCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
 
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
@@ -172,17 +172,17 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   }
 }
 
-void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) {
+void Guidance::refRotationRate(int8_t timeElapsedMax, 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 +
+      (timeSavedQuaternion.tv_sec +
-       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
+       timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
-  if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
+  if (timeElapsed < timeElapsedMax) {
     double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
+    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
     VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
 
     double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
@@ -197,21 +197,21 @@ void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double
     VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
 
     VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
+    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
-    omegaRefSavedNadir[0] = omegaRefNew[0];
+    omegaRefSaved[0] = omegaRefNew[0];
-    omegaRefSavedNadir[1] = omegaRefNew[1];
+    omegaRefSaved[1] = omegaRefNew[1];
-    omegaRefSavedNadir[2] = omegaRefNew[2];
+    omegaRefSaved[2] = omegaRefNew[2];
   } else {
     refSatRate[0] = 0;
     refSatRate[1] = 0;
     refSatRate[2] = 0;
   }
 
-  timeSavedQuaternionNadir = now;
+  timeSavedQuaternion = now;
-  savedQuaternionNadir[0] = quatInertialTarget[0];
+  savedQuaternion[0] = quatInertialTarget[0];
-  savedQuaternionNadir[1] = quatInertialTarget[1];
+  savedQuaternion[1] = quatInertialTarget[1];
-  savedQuaternionNadir[2] = quatInertialTarget[2];
+  savedQuaternion[2] = quatInertialTarget[2];
-  savedQuaternionNadir[3] = quatInertialTarget[3];
+  savedQuaternion[3] = quatInertialTarget[3];
 }
 
 void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
@@ -226,8 +226,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   double targetCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
   //	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;
@@ -286,7 +286,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -306,8 +307,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
   double groundStationCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart);
+      acsParameters.targetModeControllerParameters.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;
@@ -363,7 +364,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -495,9 +497,9 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::
 
   //	rotation quaternion from two vectors
   double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
+  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
+  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2];
+  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
   double noramlizedTargetDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
   VectorOperations<double>::normalize(refDir, refDir, 3);
@@ -576,7 +578,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  refRotationRate(now, quatInertialTarget, refSatRate);
+  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
 
   // Transform in system relative to satellite frame
   double quatBJ[4] = {0, 0, 0, 0};
@@ -586,20 +589,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
 
 void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
   for (int i = 0; i < 4; i++) {
-    targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
+    targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuat[i];
   }
   for (int i = 0; i < 3; i++) {
-    refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
+    refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRate[i];
   }
 }
 
-void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3],
                           double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
-  double quatRef[4] = {0, 0, 0, 0};
-  quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
-  quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
-  quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
-  quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
 
   double satRate[3] = {0, 0, 0};
   std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));

mission/controller/acs/Guidance.h

@@ -58,10 +58,10 @@ class Guidance {
 
   // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
   // desired
-  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
+  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3],
                   double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
 
-  void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate);
+  void refRotationRate(int8_t timeElapsedMax, 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"
@@ -70,9 +70,9 @@ class Guidance {
  private:
   AcsParameters acsParameters;
   bool strBlindAvoidFlag = false;
-  timeval timeSavedQuaternionNadir;
+  timeval timeSavedQuaternion;
-  double savedQuaternionNadir[4] = {0, 0, 0, 0};
+  double savedQuaternion[4] = {0, 0, 0, 0};
-  double omegaRefSavedNadir[3] = {0, 0, 0};
+  double omegaRefSaved[3] = {0, 0, 0};
 };
 
 #endif /* ACS_GUIDANCE_H_ */

mission/controller/acs/control/PtgCtrl.cpp

@@ -21,7 +21,8 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
 PtgCtrl::~PtgCtrl() {}
 
 void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
-  pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
+	// TODO: Here correct Parameters have to be loaded according to current submode
+  pointingLawParameters = &(acsParameters_->targetModeControllerParameters);
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
   rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
   rwMatrices = &(acsParameters_->rwMatrices);
@@ -32,10 +33,10 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
   //------------------------------------------------------------------------------------------------
-  double om = pointingModeControllerParameters->om;
+  double om = pointingLawParameters->om;
-  double zeta = pointingModeControllerParameters->zeta;
+  double zeta = pointingLawParameters->zeta;
-  double qErrorMin = pointingModeControllerParameters->qiMin;
+  double qErrorMin = pointingLawParameters->qiMin;
-  double omMax = pointingModeControllerParameters->omMax;
+  double omMax = pointingLawParameters->omMax;
 
   double cInt = 2 * om * zeta;
   double kInt = 2 * pow(om, 2);
@@ -110,7 +111,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
 void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
                               int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
                               int32_t *speedRw3, double *mgtDpDes) {
-  if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
+  if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
     mgtDpDes[0] = 0;
     mgtDpDes[1] = 0;
     mgtDpDes[2] = 0;
@@ -129,12 +130,12 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
   //	calculating momentum error
   double deltaMomentum[3] = {0, 0, 0};
   VectorOperations<double>::subtract(
-      momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
+      momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3);
   //	resulting magnetic dipole command
   double crossMomentumMagField[3] = {0, 0, 0};
   VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
   double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
-  factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
+  factor = (pointingLawParameters->deSatGainFactor) / normMag;
   VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
 }
 
@@ -150,7 +151,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
   VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
   VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
                                       wheelMomentum, 4);
-  double gainNs = pointingModeControllerParameters->gainNullspace;
+  double gainNs = pointingLawParameters->gainNullspace;
   double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
                                                 *nullSpaceMatrix, 4);

mission/controller/acs/control/PtgCtrl.h

@@ -59,7 +59,7 @@ class PtgCtrl {
   void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
 
  private:
-  AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
+  AcsParameters::PointingLawParameters *pointingLawParameters;
   AcsParameters::RwHandlingParameters *rwHandlingParameters;
   AcsParameters::InertiaEIVE *inertiaEIVE;
   AcsParameters::RwMatrices *rwMatrices;