added saftCtrl to acsController

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:
@@ -72,7 +73,66 @@ void AcsController::performControlOperation() {
   // DEBUG END
 }
 
-void AcsController::performSafe() {}
+void AcsController::performSafe() {
+  //	Concept: SAFE MODE WITH MEKF
+  //	-do the sensor processing, maybe is does make more sense do call this class function in
+  //	another place since we have to do it for every mode regardless of safe or not
+
+  ACS::SensorValues sensorValues;
+  ACS::OutputValues outputValues;
+
+  timeval now;  // We need to give the actual time here
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  ReturnValue_t validMekf;
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);  // DOES THIS WORK WITH VALID?
+  // 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 == returnvalue::OK) {
+    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);
+  }
+
+  double dipolCmdUnits[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
+
+  //	Detumble check and switch
+  if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
+                                           acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
+  }
+
+  else if (outputValues.satRateEstValid &&
+           VectorOperations<double>::norm(outputValues.satRateEst, 3) >
+               acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
+
+  }
+
+  else {
+    detumbleCounter = 0;
+  }
+
+  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
+    submode = SUBMODE_DETUMBLE;
+    detumbleCounter = 0;
+  }
+
+  //	commanding.magnetorquesDipol();
+}
 
 void AcsController::performDetumble() {
   ACS::SensorValues sensorValues;
@@ -293,6 +353,8 @@ void AcsController::copySusData() {
     PoolReadGuard pg(&susSets[9]);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
+      sif::debug << susData.sus9.isValid() << std::endl;
+      sif::debug << susSets[9].channels.isValid() << std::endl;
     }
   }
   {

mission/controller/AcsController.h

@@ -11,6 +11,7 @@
 #include "acs/SensorProcessing.h"
 #include "acs/control/Detumble.h"
 #include "acs/control/PtgCtrl.h"
+#include "acs/control/SafeCtrl.h"
 #include "controllerdefinitions/AcsCtrlDefinitions.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
@@ -29,7 +30,6 @@ class AcsController : public ExtendedControllerBase {
   static const Submode_t SUBMODE_PTG_NADIR = 5;
 
  protected:
-
   void performSafe();
   void performDetumble();
   void performPointingCtrl();
@@ -41,6 +41,7 @@ class AcsController : public ExtendedControllerBase {
   ActuatorCmd actuatorCmd;
   Guidance guidance;
 
+  SafeCtrl safeCtrl;
   Detumble detumble;
   PtgCtrl ptgCtrl;
 

mission/controller/acs/control/SafeCtrl.cpp

@@ -6,37 +6,34 @@
  */
 
 #include "SafeCtrl.h"
-#include "../util/MathOperations.h"
+
-#include <math.h>
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
 
+#include "../util/MathOperations.h"
 
-SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){
+SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
   loadAcsParameters(acsParameters_);
-	MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3);
+  MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
+                                           3);
 }
 
-SafeCtrl::~SafeCtrl(){
+SafeCtrl::~SafeCtrl() {}
 
-}
+void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
-
-void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
   safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
 }
 
 ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
                                  double *magFieldModel, bool *magFieldModelValid,
-		double *sunDirModel, bool *sunDirModelValid,
+                                 double *sunDirModel, bool *sunDirModelValid, double *satRateMekf,
-		double *satRateMekf, bool *rateMekfValid,
+                                 bool *rateMekfValid, double *sunDirRef, double *satRatRef,
-		double *sunDirRef, double *satRatRef,
+                                 double *outputMagMomB, bool *outputValid) {
-		double *outputMagMomB, bool *outputValid){
+  if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) {
-
-	if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
-			!(*rateMekfValid)) {
     *outputValid = false;
     return SAFECTRL_MEKF_INPUT_INVALID;
   }
@@ -45,10 +42,10 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
   kRate = safeModeControllerParameters->k_rate_mekf;
   kAlign = safeModeControllerParameters->k_align_mekf;
 
-//	Calc sunDirB ,magFieldB with mekf output and model
+  //	Calc sunDirB ,magFieldB with mekf output and model
-	double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
+  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
-	double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0};
+  double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
 
@@ -57,71 +54,66 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
   VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
   double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
 
-//	calc angle alpha between sunDirRef and sunDIr
+  //	calc angle alpha between sunDirRef and sunDIr
   double alpha = 0, dotSun = 0;
   dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
   alpha = acos(dotSun);
 
-//	Law Torque calculations
+  //	Law Torque calculations
-	double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0},
+  double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
-			torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0};
+         torqueAll[3] = {0, 0, 0};
 
   double scalarFac = 0;
   scalarFac = kAlign * alpha / normCrossSun;
   VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
 
-	double rateSafeMode[3] = {0,0,0};
+  double rateSafeMode[3] = {0, 0, 0};
   VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
   VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
 
   VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
-//	Adding factor of inertia for axes
+  //	Adding factor of inertia for axes
   MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
 
-// MagMom B (orthogonal torque)
+  // MagMom B (orthogonal torque)
-	double torqueMgt[3] = {0,0,0};
+  double torqueMgt[3] = {0, 0, 0};
   VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
   double normMag = VectorOperations<double>::norm(magFieldB, 3);
-	VectorOperations<double>::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3);
+  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
   *outputValid = true;
 
   return returnvalue::OK;
-
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
-void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
+void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
-		double *sunRateB, bool *sunRateBValid,
+                          bool *sunRateBValid, double *magFieldB, bool *magFieldBValid,
-		double *magFieldB, bool *magFieldBValid,
+                          double *magRateB, bool *magRateBValid, double *sunDirRef,
-		double *magRateB, bool *magRateBValid,
+                          double *satRateRef, double *outputMagMomB, bool *outputValid) {
-		double *sunDirRef, double *satRateRef,
+  // Check for invalid Inputs
-		double *outputMagMomB, bool *outputValid){
+  if (!susDirBValid || !magFieldBValid || !magRateBValid) {
-
-// Check for invalid Inputs
-	if ( !susDirBValid || !magFieldBValid || !magRateBValid) {
     *outputValid = false;
     return;
   }
 
-//	normalize sunDir and magDir
+  //	normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(magFieldB, magDirB, 3);
   VectorOperations<double>::normalize(susDirB, susDirB, 3);
 
-//	Cosinus angle between sunDir and magDir
+  //	Cosinus angle between sunDir and magDir
   double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
 
-//	Rate parallel to sun direction and magnetic field direction
+  //	Rate parallel to sun direction and magnetic field direction
   double rateParaSun = 0, rateParaMag = 0;
-	double dotSunRateMag = 0, dotmagRateSun = 0,
+  double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
-			rateFactor = 0;
   dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
   dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
-	rateFactor = 1 - pow(cosAngleSunMag,2);
+  rateFactor = 1 - pow(cosAngleSunMag, 2);
-	rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor;
+  rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
-	rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor;
+  rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
 
-//	Full rate or estimate
+  //	Full rate or estimate
   double estSatRate[3] = {0, 0, 0};
   double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
@@ -131,21 +123,21 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
   VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
   VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
 
-/*    Only valid if angle between sun direction and magnetic field direction
+  /*    Only valid if angle between sun direction and magnetic field direction
         is sufficiently large */
 
   double sinAngle = 0;
   sinAngle = sin(acos(cos(cosAngleSunMag)));
 
-    if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
+  if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
     return;
   }
 
-//    Rate for Torque Calculation
+  //    Rate for Torque Calculation
   double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
   VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
 
-//    Torque Align calculation
+  //    Torque Align calculation
   double kRateNoMekf = 0, kAlignNoMekf = 0;
   kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
   kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
@@ -160,28 +152,26 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
   double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
   VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
 
-//Torque Rate Calculations
+  // Torque Rate Calculations
   double torqueRate[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
 
-//Final torque
+  // Final torque
   double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
   VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
-    MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1);
+  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
+                                     1);
 
-//Magnetic moment
+  // Magnetic moment
   double magMomB[3] = {0, 0, 0};
   double crossMagFieldTorque[3] = {0, 0, 0};
   VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
-    double magMomFactor = pow( VectorOperations<double>::norm(magFieldB, 3), 2 );
+  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
-    VectorOperations<double>::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3);
+  VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
 
   outputMagMomB[0] = magMomB[0];
   outputMagMomB[1] = magMomB[1];
   outputMagMomB[2] = magMomB[2];
 
   *outputValid = true;
-
 }
-
-

mission/controller/acs/control/SafeCtrl.h

@@ -8,20 +8,17 @@
 #ifndef SAFECTRL_H_
 #define SAFECTRL_H_
 
-#include "../SensorValues.h"
-#include "../OutputValues.h"
-#include "../AcsParameters.h"
-#include "../config/classIds.h"
-#include <string.h>
 #include <stdio.h>
+#include <string.h>
 #include <time.h>
-#include <fsfw/returnvalues/HasReturnvaluesIF.h>
 
+#include "../AcsParameters.h"
+#include "../OutputValues.h"
+#include "../SensorValues.h"
+#include "../config/classIds.h"
 
-class SafeCtrl{
+class SafeCtrl {
-
+ public:
-public:
-
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
@@ -30,35 +27,27 @@ public:
 
   void loadAcsParameters(AcsParameters *acsParameters_);
 
-	ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
+  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel,
-			double *magFieldModel, bool *magFieldModelValid,
+                         bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid,
-			double *sunDirModel, bool *sunDirModelValid,
+                         double *satRateMekf, bool *rateMekfValid, double *sunDirRef,
-			double *satRateMekf, bool *rateMekfValid,
+                         double *satRatRef,  // From Guidance (!)
-			double *sunDirRef, double *satRatRef, // From Guidance (!)
                          double *outputMagMomB, bool *outputValid);
 
-	void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
+  void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
-			double *sunRateB, bool *sunRateBValid,
+                  bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB,
-			double *magFieldB, bool *magFieldBValid,
+                  bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB,
-			double *magRateB, bool *magRateBValid,
+                  bool *outputValid);
-			double *sunDirRef, double *satRateRef,
-			double *outputMagMomB, bool *outputValid);
 
   void idleSunPointing();  // with reaction wheels
 
-protected:
+ protected:
-
+ private:
-private:
+  AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
-	AcsParameters::SafeModeControllerParameters* safeModeControllerParameters;
+  AcsParameters::InertiaEIVE *inertiaEIVE;
-	AcsParameters::InertiaEIVE* inertiaEIVE;
   double gainMatrixInertia[3][3];
 
   double magFieldBState[3];
   timeval magFieldBStateTime;
-
 };
 
-
-
 #endif /* ACS_CONTROL_SAFECTRL_H_ */
-