added Antistiction, added Nadir Pointing, added performSafe()

mission/controller/AcsController.cpp

@@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId)
       navigation(&acsParameters),
       actuatorCmd(&acsParameters),
       guidance(&acsParameters),
+	  safeCtrl(&acsParameters),
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
       detumbleCounter{0},
@@ -35,7 +36,7 @@ void AcsController::performControlOperation() {
       if (mode != MODE_OFF) {
         switch (submode) {
           case SUBMODE_SAFE:
-            // performSafe();
+            performSafe();
             break;
 
           case SUBMODE_DETUMBLE:
@@ -45,9 +46,14 @@ void AcsController::performControlOperation() {
           case SUBMODE_PTG_GS:
             performPointingCtrl();
             break;
+
           case SUBMODE_PTG_SUN:
-             performPointingCtrlSun();
+        	performPointingCtrl();
-             break;
+            break;
+
+          case SUBMODE_PTG_NADIR:
+        	performPointingCtrl();
+            break;
         }
       }
       break;
@@ -75,7 +81,40 @@ void AcsController::performControlOperation() {
   // DEBUG END
 }
 
-void AcsController::performSafe() {}
+void AcsController::performSafe() {
+  ACS::SensorValues sensorValues;
+  ACS::OutputValues outputValues;
+
+  timeval now;
+  Clock::getClock_timeval(&now);
+
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  ReturnValue_t validMekf;
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+	// Give desired satellite rate and sun direction to align
+	double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
+	guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
+//	IF MEKF is working
+	double magMomMtq[3] = {0, 0, 0};
+	bool magMomMtqValid = false;
+	if ( !validMekf ) { // Valid = 0, Failed = 1
+		safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
+				(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
+				(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
+				(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
+				sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
+	}
+	else {
+
+		safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
+				outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
+				outputValues.magFieldEst, &(outputValues.magFieldEstValid),
+				outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
+				sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
+
+	}
+
+}
 
 void AcsController::performDetumble() {
   ACS::SensorValues sensorValues;
@@ -121,7 +160,18 @@ void AcsController::performPointingCtrl() {
   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);
+
+  switch (submode) {
+    case SUBMODE_PTG_GS:
+      guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+      break;
+    case SUBMODE_PTG_SUN:
+      guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
+      break;
+    case SUBMODE_PTG_NADIR:
+      guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+      break;
+  }
   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}};
@@ -132,45 +182,21 @@ void AcsController::performPointingCtrl() {
   ptgCtrl.ptgNullspace(
       &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
       &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+  double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
+  VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
+  actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
+
+  if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
+	  bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
+  	  double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
+	      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)};
+  	  ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
+  }
+
   double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
   actuatorCmd.cmdSpeedToRws(
       &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
+      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
-      rwTrqNs, cmdSpeedRws);
-  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
-  ptgCtrl.ptgDesaturation(
-      outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
-  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
-}
-
-void AcsController::performPointingCtrlSun() {
-  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.sunQuatPtg(&sensorValues, &outputValues, 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.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
-  double rwTrqNs[4] = {0, 0, 0, 0};
-  ptgCtrl.ptgNullspace(
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
-  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
-  actuatorCmd.cmdSpeedToRws(
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
-      rwTrqNs, cmdSpeedRws);
   double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
   ptgCtrl.ptgDesaturation(
       outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,

mission/controller/AcsController.h

@@ -9,6 +9,7 @@
 #include "acs/Guidance.h"
 #include "acs/Navigation.h"
 #include "acs/SensorProcessing.h"
+#include "acs/control/SafeCtrl.h"
 #include "acs/control/Detumble.h"
 #include "acs/control/PtgCtrl.h"
 #include "controllerdefinitions/AcsCtrlDefinitions.h"
@@ -34,7 +35,6 @@ class AcsController : public ExtendedControllerBase {
   void performSafe();
   void performDetumble();
   void performPointingCtrl();
-  void performPointingCtrlSun();
 
  private:
   AcsParameters acsParameters;
@@ -43,6 +43,7 @@ class AcsController : public ExtendedControllerBase {
   ActuatorCmd actuatorCmd;
   Guidance guidance;
 
+  SafeCtrl safeCtrl;
   Detumble detumble;
   PtgCtrl ptgCtrl;
 

mission/controller/acs/AcsParameters.h

@@ -776,6 +776,9 @@ class AcsParameters /*: public HasParametersIF*/ {
     double rw3orientationMatrix[3][3];
     double inertiaWheel = 0.000028198;
     double maxTrq = 0.0032;  // 3.2 [mNm]
+    double stictionSpeed = 80; //RPM
+    double stictionReleaseSpeed = 120; //RPM
+    double stictionTorque = 0.0006;
   } rwHandlingParameters;
 
   struct RwMatrices {
@@ -842,10 +845,12 @@ class AcsParameters /*: public HasParametersIF*/ {
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
     bool desatOn = true;
+    bool enableAntiStiction = true;
 
     double omegaEarth = 0.000072921158553;
 
-  } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
+    double nadirRefDirection[3] = {-1, 0, 0}; //Camera
+  } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
 
   struct StrParameters {
     double exclusionAngle = 20 * M_PI / 180;

mission/controller/acs/ActuatorCmd.cpp

@@ -23,26 +23,27 @@ ActuatorCmd::~ActuatorCmd(){
 
 }
 
-void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
+void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
-                                const int32_t *speedRw2, const int32_t *speedRw3,
-                                const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
-  using namespace Math;
   // Scaling the commanded torque to a maximum value
-  double torque[4] = {0, 0, 0, 0};
   double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
-  VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
 
   double maxValue = 0;
   for (int i = 0; i < 4; i++) {  // size of torque, always 4 ?
-    if (abs(torque[i]) > maxValue) {
+    if (abs(rwTrq[i]) > maxValue) {
-      maxValue = abs(torque[i]);
+      maxValue = abs(rwTrq[i]);
     }
   }
 
   if (maxValue > maxTrq) {
-    double scalingFactor = maxTrq / maxValue;
+  double scalingFactor = maxTrq / maxValue;
-    VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
+  VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
   }
+}
+
+void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
+                                const int32_t *speedRw2, const int32_t *speedRw3,
+                                const double *rwTorque, double *rwCmdSpeed) {
+  using namespace Math;
 
   // Calculating the commanded speed in RPM for every reaction wheel
   double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
@@ -52,7 +53,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
   double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
   //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
   double factor = commandTime / inertiaWheel * radToRpm;
-  VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
+  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
   VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
 }
 

mission/controller/acs/ActuatorCmd.h

@@ -20,6 +20,14 @@ public:
 	ActuatorCmd(AcsParameters *acsParameters_); //Input mode ?
 	virtual ~ActuatorCmd();
 
+	/*
+	 * @brief:	scalingTorqueRws()	scales the torque via maximum part in case this part is higher
+	 * 								then the maximum torque
+	 * @param:	rwTrq	 			given torque for reaction wheels
+	 * 			rwTrqScaled			possible scaled torque
+	 */
+        void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
+
 	/*
 	 * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction wheels, also
 	 * 								will calculate the needed revolutions per minute for the RWs, which will be given
@@ -29,8 +37,8 @@ public:
 	 * 			rwCmdSpeed			output revolutions per minute for every reaction wheel
 	 */
         void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
-                           const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn,
+                           const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTorque,
-                           const double *rwTrqNS, double *rwCmdSpeed);
+						   double *rwCmdSpeed);
 
         /*
 	 * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques

mission/controller/acs/Guidance.cpp

@@ -234,6 +234,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
   refSatRate[2] = 0;
 }
 
+void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
+	double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for Nadir pointing
+  //-------------------------------------------------------------------------------------
+  //	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>::mulScalar(posSatE, -1, 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);
+
+  //	Transformation between ECEF and Body frame
+  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];
+  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);
+
+  //	Target Direction in the body frame
+  double targetDirB[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
+
+  //	rotation quaternion from two vectors
+  double refDir[3] = {0, 0, 0};
+  refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
+  refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
+  refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2];
+  double noramlizedTargetDirB[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
+  VectorOperations<double>::normalize(refDir, refDir, 3);
+  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
+  double normRefDir = VectorOperations<double>::norm(refDir, 3);
+  double crossDir[3] = {0, 0, 0};
+  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
+  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
+  targetQuat[0] = crossDir[0];
+  targetQuat[1] = crossDir[1];
+  targetQuat[2] = crossDir[2];
+  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
+  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
+
+  //-------------------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //-------------------------------------------------------------------------------------
+  refSatRate[0] = 0;
+  refSatRate[1] = 0;
+  refSatRate[2] = 0;
+
+}
+
 void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
                           double refSatRate[3], double quatError[3], double deltaRate[3]) {
   double quatRef[4] = {0, 0, 0, 0};

mission/controller/acs/Guidance.h

@@ -30,6 +30,10 @@ public:
 	void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
 			double targetQuat[4], double refSatRate[3]);
 
+	// Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing
+	void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
+			double targetQuat[4], double refSatRate[3]);
+
 	// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
 	void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );
 

mission/controller/acs/control/PtgCtrl.cpp

@@ -16,7 +16,7 @@
 #include <fsfw/globalfunctions/sign.h>
 #include <math.h>
 
-PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
+PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
 	loadAcsParameters(acsParameters_);
 }
 
@@ -164,3 +164,36 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
   VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
   VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
 }
+
+void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw,
+		double* torqueCommand) {
+	for (uint8_t i = 0; i < 4; i++) {
+			if (rwAvailable[i]) {
+				if (torqueMemory[i] != 0) {
+					if ((omegaRw[i] * torqueMemory[i])
+							> rwHandlingParameters->stictionReleaseSpeed) {
+						torqueMemory[i] = 0;
+					} else {
+						torqueCommand[i] = torqueMemory[i]
+								* rwHandlingParameters->stictionTorque;
+					}
+				} else {
+					if ((omegaRw[i] < rwHandlingParameters->stictionSpeed)
+							&& (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
+						if (omegaRw[i] < omegaMemory[i]) {
+							torqueMemory[i] = -1;
+						} else {
+							torqueMemory[i] = 1;
+						}
+
+						torqueCommand[i] = torqueMemory[i]
+								* rwHandlingParameters->stictionTorque;
+					}
+				}
+			} else {
+				torqueMemory[i] = 0;
+			}
+			omegaMemory[i] = omegaRw[i];
+		}
+	}
+}

mission/controller/acs/control/PtgCtrl.h

@@ -52,11 +52,22 @@ class PtgCtrl {
   void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
                     const int32_t *speedRw3, double *rwTrqNs);
 
+  /* @brief: Commands the stiction torque in case wheel speed is to low
+   * @param: rwAvailable 	Boolean Flag for all reaction wheels
+   * 		 omegaRw		current wheel speed of reaction wheels
+   * 		 torqueCommand  modified torque after antistiction
+   */
+  void rwAntistiction(const bool* rwAvailable, const double* omegaRw,
+			double* torqueCommand);
+
  private:
   AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
   AcsParameters::RwHandlingParameters *rwHandlingParameters;
   AcsParameters::InertiaEIVE *inertiaEIVE;
   AcsParameters::RwMatrices *rwMatrices;
+
+  double torqueMemory[4];
+  double omegaMemory[4];
 };
 
 #endif /* ACS_CONTROL_PTGCTRL_H_ */