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,182 +6,172 @@
  */
 
 #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_);
+  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_) {
-
+  safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
-void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
+  inertiaEIVE = &(acsParameters_->inertiaEIVE);
-	safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
-	inertiaEIVE = &(acsParameters_->inertiaEIVE);
 }
 
 ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
-		double *magFieldModel, bool *magFieldModelValid,
+                                 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)) {
+    *outputValid = false;
+    return SAFECTRL_MEKF_INPUT_INVALID;
+  }
 
-	if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
+  double kRate = 0, kAlign = 0;
-			!(*rateMekfValid)) {
+  kRate = safeModeControllerParameters->k_rate_mekf;
-		*outputValid = false;
+  kAlign = safeModeControllerParameters->k_align_mekf;
-		return SAFECTRL_MEKF_INPUT_INVALID;
-	}
 
-	double kRate = 0, kAlign = 0;
+  //	Calc sunDirB ,magFieldB with mekf output and model
-	kRate = safeModeControllerParameters->k_rate_mekf;
+  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-	kAlign = safeModeControllerParameters->k_align_mekf;
+  MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
+  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);
 
-//	Calc sunDirB ,magFieldB with mekf output and model
+  double crossSun[3] = {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};
-	MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
-	MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
 
-	double crossSun[3] = {0, 0, 0};
+  VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
+  double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
 
-	VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
+  //	calc angle alpha between sunDirRef and sunDIr
-	double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
+  double alpha = 0, dotSun = 0;
+  dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
+  alpha = acos(dotSun);
 
-//	calc angle alpha between sunDirRef and sunDIr
+  //	Law Torque calculations
-	double alpha = 0, dotSun = 0;
+  double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
-	dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
+         torqueAll[3] = {0, 0, 0};
-	alpha = acos(dotSun);
 
-//	Law Torque calculations
+  double scalarFac = 0;
-	double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0},
+  scalarFac = kAlign * alpha / normCrossSun;
-			torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
 
-	double scalarFac = 0;
+  double rateSafeMode[3] = {0, 0, 0};
-	scalarFac = kAlign * alpha / normCrossSun;
+  VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
-	VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
+  VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
 
-	double rateSafeMode[3] = {0,0,0};
+  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
-	VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
+  //	Adding factor of inertia for axes
-	VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
+  MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
 
-	VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
+  // MagMom B (orthogonal torque)
-//	Adding factor of inertia for axes
+  double torqueMgt[3] = {0, 0, 0};
-	MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
+  VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
-
+  double normMag = VectorOperations<double>::norm(magFieldB, 3);
-// MagMom B (orthogonal torque)
+  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
-	double torqueMgt[3] = {0,0,0};
+  *outputValid = true;
-	VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
-	double normMag = VectorOperations<double>::norm(magFieldB, 3);
-	VectorOperations<double>::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3);
-	*outputValid = true;
-
-	return returnvalue::OK;
 
+  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) {
+    *outputValid = false;
+    return;
+  }
 
-// Check for invalid Inputs
+  //	normalize sunDir and magDir
-	if ( !susDirBValid || !magFieldBValid || !magRateBValid) {
+  double magDirB[3] = {0, 0, 0};
-		*outputValid = false;
+  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
-		return;
+  VectorOperations<double>::normalize(susDirB, susDirB, 3);
-	}
 
-//	normalize sunDir and magDir
+  //	Cosinus angle between sunDir and magDir
-	double magDirB[3] = {0, 0, 0};
+  double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
-	VectorOperations<double>::normalize(magFieldB, magDirB, 3);
-	VectorOperations<double>::normalize(susDirB, susDirB, 3);
 
-//	Cosinus angle between sunDir and magDir
+  //	Rate parallel to sun direction and magnetic field direction
-	double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
+  double rateParaSun = 0, rateParaMag = 0;
+  double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
+  dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
+  dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
+  rateFactor = 1 - pow(cosAngleSunMag, 2);
+  rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
+  rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
 
-//	Rate parallel to sun direction and magnetic field direction
+  //	Full rate or estimate
-	double rateParaSun = 0, rateParaMag = 0;
+  double estSatRate[3] = {0, 0, 0};
-	double dotSunRateMag = 0, dotmagRateSun = 0,
+  double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
-			rateFactor = 0;
+  VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
-	dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
+  VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
-	dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
+  VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
-	rateFactor = 1 - pow(cosAngleSunMag,2);
+  VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
-	rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor;
+  VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
-	rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor;
+  VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
 
-//	Full rate or estimate
+  /*    Only valid if angle between sun direction and magnetic field direction
-	double estSatRate[3] = {0, 0, 0};
+        is sufficiently large */
-	double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
-    VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
-    VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
-    VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
-    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
+  double sinAngle = 0;
-      is sufficiently large */
+  sinAngle = sin(acos(cos(cosAngleSunMag)));
 
-    double sinAngle = 0;
+  if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
-    sinAngle = sin(acos(cos(cosAngleSunMag)));
+    return;
+  }
 
-    if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
+  //    Rate for Torque Calculation
-    	return;
+  double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
-    }
+  VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
 
-//    Rate for Torque Calculation
+  //    Torque Align calculation
-    double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
+  double kRateNoMekf = 0, kAlignNoMekf = 0;
-    VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
+  kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
+  kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
 
-//    Torque Align calculation
+  double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
-	double kRateNoMekf = 0, kAlignNoMekf = 0;
+  double crossSusSunRef[3] = {0, 0, 0};
-	kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
+  VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
-	kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
+  double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
 
-    double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
+  double torqueAlign[3] = {0, 0, 0};
-    double crossSusSunRef[3] = {0, 0, 0};
+  double angleAlignErr = acos(cosAngleAlignErr);
-    VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
+  double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
-    double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
+  VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
 
-    double torqueAlign[3] = {0, 0, 0};
+  // Torque Rate Calculations
-    double angleAlignErr = acos(cosAngleAlignErr);
+  double torqueRate[3] = {0, 0, 0};
-    double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
+  VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
-    VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
 
-//Torque Rate Calculations
+  // Final torque
-    double torqueRate[3] = {0, 0, 0};
+  double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
+  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
+  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
+                                     1);
 
-//Final torque
+  // Magnetic moment
-    double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
+  double magMomB[3] = {0, 0, 0};
-    VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
+  double crossMagFieldTorque[3] = {0, 0, 0};
-    MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1);
+  VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
+  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
+  VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
 
-//Magnetic moment
+  outputMagMomB[0] = magMomB[0];
-    double magMomB[3] = {0, 0, 0};
+  outputMagMomB[1] = magMomB[1];
-    double crossMagFieldTorque[3] = {0, 0, 0};
+  outputMagMomB[2] = magMomB[2];
-    VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
-    double magMomFactor = pow( VectorOperations<double>::norm(magFieldB, 3), 2 );
-    VectorOperations<double>::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3);
-
-    outputMagMomB[0] = magMomB[0];
-    outputMagMomB[1] = magMomB[1];
-    outputMagMomB[2] = magMomB[2];
-
-    *outputValid = true;
 
+  *outputValid = true;
 }
-
-

mission/controller/acs/control/SafeCtrl.h

@@ -8,57 +8,46 @@
 #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:
+  SafeCtrl(AcsParameters *acsParameters_);
+  virtual ~SafeCtrl();
 
-public:
+  static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
+  static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
 
-	SafeCtrl(AcsParameters *acsParameters_);
+  void loadAcsParameters(AcsParameters *acsParameters_);
-	virtual ~SafeCtrl();
 
-	static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
+  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel,
-	static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
+                         bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid,
+                         double *satRateMekf, bool *rateMekfValid, double *sunDirRef,
+                         double *satRatRef,  // From Guidance (!)
+                         double *outputMagMomB, bool *outputValid);
 
-	void loadAcsParameters(AcsParameters *acsParameters_);
+  void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
+                  bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB,
+                  bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB,
+                  bool *outputValid);
 
-	ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
+  void idleSunPointing();  // with reaction wheels
-			double *magFieldModel, bool *magFieldModelValid,
-			double *sunDirModel, bool *sunDirModelValid,
-			double *satRateMekf, bool *rateMekfValid,
-			double *sunDirRef, double *satRatRef, // From Guidance (!)
-			double *outputMagMomB, bool *outputValid);
 
-	void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
+ protected:
-			double *sunRateB, bool *sunRateBValid,
+ private:
-			double *magFieldB, bool *magFieldBValid,
+  AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
-			double *magRateB, bool *magRateBValid,
+  AcsParameters::InertiaEIVE *inertiaEIVE;
-			double *sunDirRef, double *satRateRef,
+  double gainMatrixInertia[3][3];
-			double *outputMagMomB, bool *outputValid);
-
-	void idleSunPointing(); // with reaction wheels
-
-protected:
-
-private:
-	AcsParameters::SafeModeControllerParameters* safeModeControllerParameters;
-	AcsParameters::InertiaEIVE* inertiaEIVE;
-	double gainMatrixInertia[3][3];
-
-	double magFieldBState[3];
-	timeval magFieldBStateTime;
 
+  double magFieldBState[3];
+  timeval magFieldBStateTime;
 };
 
-
-
 #endif /* ACS_CONTROL_SAFECTRL_H_ */
-