updated AcsParameters. change DCM_EJ calc to with precission and nutation

mission/controller/AcsController.cpp

@@ -45,6 +45,9 @@ void AcsController::performControlOperation() {
           case SUBMODE_PTG_GS:
             performPointingCtrl();
             break;
+          case SUBMODE_PTG_SUN:
+             performPointingCtrlSun();
+             break;
         }
       }
       break;
@@ -124,7 +127,41 @@ void AcsController::performPointingCtrl() {
   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);
+  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,
+      &(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),

mission/controller/AcsController.h

@@ -27,12 +27,14 @@ class AcsController : public ExtendedControllerBase {
   static const Submode_t SUBMODE_DETUMBLE = 3;
   static const Submode_t SUBMODE_PTG_GS = 4;
   static const Submode_t SUBMODE_PTG_NADIR = 5;
+  static const Submode_t SUBMODE_PTG_SUN = 6;
 
  protected:
 
   void performSafe();
   void performDetumble();
   void performPointingCtrl();
+  void performPointingCtrlSun();
 
  private:
   AcsParameters acsParameters;

mission/controller/acs/AcsParameters.h

@@ -29,7 +29,11 @@ class AcsParameters /*: public HasParametersIF*/ {
     double inertiaMatrix[3][3] = 	{{0.1539829, -0.0001821456, -0.0050135},
     								{-0.0001821456, 0.1701302, 0.0004748963},
 									{-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021
-    double inertiaMatrixNoPanel[3][3] = 	{{0.122485, -0.0001798426, -0.005008},
+    // Possible inertia matrices
+    double inertiaMatrixDeployed[3][3] = 	{{0.1539829, -0.0001821456, -0.0050135},
+     								{-0.0001821456, 0.1701302, 0.0004748963},
+ 									{-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021
+    double inertiaMatrixUndeployed[3][3] = 	{{0.122485, -0.0001798426, -0.005008},
 											{-0.0001798426, 0.162240, 0.000475596},
 											{-0.005008, 0.000475596, 0.060136}}; //19.11.2021
     double inertiaMatrixPanel1[3][3] = 	{{0.13823347, -0.0001836122, -0.00501207},
@@ -37,7 +41,7 @@ class AcsParameters /*: public HasParametersIF*/ {
 											{-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021
     double inertiaMatrixPanel3[3][3] = 	{{0.13823487, -0.000178376, -0.005009767},
  											{-0.000178376, 0.166172, -0.007403},
- 											{-0.005009767, -0.007403, 0.07195314}}; //19.11.2021
+ 											{-0.005009767, -0.007403, 0.07195314}};
   } inertiaEIVE;
 
   struct MgmHandlingParameters {
@@ -821,7 +825,7 @@ class AcsParameters /*: public HasParametersIF*/ {
 
   // ToDo: mutiple structs for different pointing mode controllers?
   struct PointingModeControllerParameters {
-    double refDirection[3] = {1, 0, 0}; //Antenna
+    double refDirection[3] = {-1, 0, 0}; //Antenna
     double refRotRate[3] = {0, 0, 0};
     double quatRef[4] = {0, 0, 0, 1};
     bool avoidBlindStr = true;
@@ -891,9 +895,9 @@ class AcsParameters /*: public HasParametersIF*/ {
   } magnetorquesParameter;
 
   struct DetumbleParameter {
-    uint8_t detumblecounter = 75;
+    uint8_t detumblecounter = 75; // 30 s
     double omegaDetumbleStart = 2 * M_PI / 180;
-    double omegaDetumbleEnd = 0.2 * M_PI / 180;
+    double omegaDetumbleEnd = 0.4 * M_PI / 180;
     double gainD = pow(10.0, -3.3);
   } detumbleParameter;
 };

mission/controller/acs/Guidance.cpp

@@ -57,21 +57,11 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   //	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}};
-  MathOperations<double>::dcmEJ(now, *dcmEJ);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-  //	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
   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}};
-  double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
-  double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
-
-  //	TEST SECTION !
-  // double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  // MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
-  // dcmTEST, dcmTEST, 3, 3, 3);
-
-  MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
   MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
 
   //	Transformation between ECEF and Body frame

mission/controller/acs/control/PtgCtrl.cpp

@@ -31,7 +31,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
 	rwMatrices =&(acsParameters_->rwMatrices);
 }
 
-void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
+void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
 
 	//------------------------------------------------------------------------------------------------
 	// Compute gain matrix K and P matrix

mission/controller/acs/control/PtgCtrl.h

@@ -42,7 +42,7 @@ class PtgCtrl {
   /* @brief: Calculates the needed torque for the pointing control mechanism
    * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
    */
-  void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
+  void ptgLaw(const double mode, const double *qError, const double *deltaRate,
                         const double *rwPseudoInv, double *torqueRws);
 
   void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,

mission/controller/acs/util/MathOperations.h

@@ -124,10 +124,11 @@ public:
 
 	}
 
-	static void dcmEJ(timeval time, T1 * outputDcmEJ){
+	static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
 		/* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
 		 * @param:  time   			Current time
 		 * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
+		 * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
 		 * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
 		 * 			Landis Markley and John L. Crassidis*/
 		double JD2000Floor = 0;
@@ -162,6 +163,16 @@ public:
 		outputDcmEJ[7] = 0;
 		outputDcmEJ[8] = 1;
 
+		//	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
+		double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
+								 {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
+								 {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
+		double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
+		double omegaEarth = 0.000072921158553;
+		double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
+		MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
+		MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
+
 	}
 
 	/* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame