afmt

dummies/GyroL3GD20Dummy.cpp

@@ -38,7 +38,6 @@ void GyroL3GD20Dummy::fillCommandAndReplyMap() {}
 
 uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }
 
-
 ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));

dummies/ImtqDummy.cpp

@@ -39,7 +39,7 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0,0.0,0.0}));
+  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
   localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
   localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));

dummies/SusDummy.cpp

@@ -37,7 +37,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPo
                                                 LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
   localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
-	                           new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0},true));
+                           new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}, true));
 
   return returnvalue::OK;
 }

mission/controller/AcsController.h

@@ -70,7 +70,7 @@ class AcsController : public ExtendedControllerBase {
   void announceMode(bool recursive);
 
   /* ACS Datasets */
-IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
+  IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
   // MGMs
   acsctrl::MgmDataRaw mgmDataRaw;
   PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

mission/controller/acs/config/classIds.h

@@ -5,7 +5,7 @@
 #include <fsfw/returnvalues/FwClassIds.h>
 
 namespace CLASS_ID {
-enum eiveclassIds: uint8_t {
+enum eiveclassIds : uint8_t {
   EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
   KALMAN,
   SAFE,
@@ -15,5 +15,4 @@ enum eiveclassIds: uint8_t {
 };
 }
 
-
 #endif /* ACS_CONFIG_CLASSIDS_H_ */

mission/controller/acs/control/CMakeLists.txt

@@ -1,5 +1,2 @@
-target_sources(
+target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
-  ${LIB_EIVE_MISSION}
-  PRIVATE Detumble.cpp
-  		  PtgCtrl.cpp
                                            SafeCtrl.cpp)

mission/controller/acs/control/Detumble.cpp

@@ -6,30 +6,24 @@
  *      Author: Robin Marquardt
  */
 
-
 #include "Detumble.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 <fsfw/globalfunctions/sign.h>
+#include <math.h>
 
+#include "../util/MathOperations.h"
 
-Detumble::Detumble(AcsParameters *acsParameters_){
+Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
-	loadAcsParameters(acsParameters_);
-}
 
-Detumble::~Detumble(){
+Detumble::~Detumble() {}
-
-}
-
-void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
 
+void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
   detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
   magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
-
 }
 
 ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,

mission/controller/acs/control/Detumble.h

@@ -8,18 +8,17 @@
 #ifndef ACS_CONTROL_DETUMBLE_H_
 #define ACS_CONTROL_DETUMBLE_H_
 
-#include "../SensorValues.h"
-#include "../AcsParameters.h"
-#include "../config/classIds.h"
-#include <string.h>
-#include <stdio.h>
-#include <time.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
 
+#include "../AcsParameters.h"
+#include "../SensorValues.h"
+#include "../config/classIds.h"
 
-class Detumble{
+class Detumble {
-
+ public:
-public:
   Detumble(AcsParameters *acsParameters_);
   virtual ~Detumble();
 
@@ -31,15 +30,14 @@ public:
    */
   void loadAcsParameters(AcsParameters *acsParameters_);
 
-        ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
+  ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
-                              const double *magField, const bool magFieldValid, double *magMom);
+                        const bool magFieldValid, double *magMom);
 
   ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
 
  private:
-	AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
+  AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
-	AcsParameters::MagnetorquesParameter* magnetorquesParameter;
+  AcsParameters::MagnetorquesParameter *magnetorquesParameter;
 };
 
 #endif /*ACS_CONTROL_DETUMBLE_H_*/
-

mission/controller/acs/control/PtgCtrl.cpp

@@ -5,10 +5,8 @@
  *      Author: Robin Marquardt
  */
 
-
-
 #include "PtgCtrl.h"
-#include "../util/MathOperations.h"
+
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
@@ -16,23 +14,21 @@
 #include <fsfw/globalfunctions/sign.h>
 #include <math.h>
 
-PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
+#include "../util/MathOperations.h"
-	loadAcsParameters(acsParameters_);
-}
 
-PtgCtrl::~PtgCtrl(){
+PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
 
-}
+PtgCtrl::~PtgCtrl() {}
 
-void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
+void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
   pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
   rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
-	rwMatrices =&(acsParameters_->rwMatrices);
+  rwMatrices = &(acsParameters_->rwMatrices);
 }
 
-void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
+void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
-
+                               const double *rwPseudoInv, double *torqueRws) {
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
   //------------------------------------------------------------------------------------------------
@@ -42,15 +38,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
   double omMax = pointingModeControllerParameters->omMax;
 
   double cInt = 2 * om * zeta;
-	double kInt = 2 * pow(om,2);
+  double kInt = 2 * pow(om, 2);
 
   double qErrorLaw[3] = {0, 0, 0};
 
   for (int i = 0; i < 3; i++) {
     if (abs(qError[i]) < qErrorMin) {
       qErrorLaw[i] = qErrorMin;
-		}
+    } else {
-		else {
       qErrorLaw[i] = abs(qError[i]);
     }
   }
@@ -65,7 +60,8 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
   gainMatrixDiagonal[0][0] = gainVector[0];
   gainMatrixDiagonal[1][1] = gainVector[1];
   gainMatrixDiagonal[2][2] = gainVector[2];
-	MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
+  MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
+                                     *gainMatrix, 3, 3, 3);
 
   // Inverse of gainMatrix
   double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -74,7 +70,8 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
   gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
 
   double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-	MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
+  MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
+                                     3, 3);
   MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
 
   //------------------------------------------------------------------------------------------------
@@ -86,30 +83,27 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
   double pErrorSign[3] = {0, 0, 0};
 
   for (int i = 0; i < 3; i++) {
-
     if (abs(pError[i]) > 1) {
       pErrorSign[i] = sign(pError[i]);
-		}
+    } else {
-		else {
       pErrorSign[i] = pError[i];
     }
   }
-//	Torque for quaternion error
+  //	Torque for quaternion error
   double torqueQuat[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
   VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
 
-//  Torque for rate error
+  //  Torque for rate error
   double torqueRate[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
   VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
   VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
 
-//	Final commanded Torque for every reaction wheel
+  //	Final commanded Torque for every reaction wheel
   double torque[3] = {0, 0, 0};
   VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
   MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
-
 }
 
 void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,

mission/controller/acs/util/CholeskyDecomposition.h

@@ -8,36 +8,35 @@
 #ifndef CHOLESKYDECOMPOSITION_H_
 #define CHOLESKYDECOMPOSITION_H_
 #include <math.h>
-//typedef unsigned int uint8_t;
+// typedef unsigned int uint8_t;
 
-template<typename T1, typename T2=T1, typename T3=T2>
+template <typename T1, typename T2 = T1, typename T3 = T2>
 class CholeskyDecomposition {
-public:
+ public:
-	static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension)
+  static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
-	{
     // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
     return cholsl(matrix, result, tempMatrix, dimension);
   }
-private:
+
+ private:
   // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	static uint8_t choldc1(double * a, double * p, uint8_t n) {
+  static uint8_t choldc1(double *a, double *p, uint8_t n) {
-		int8_t i,j,k;
+    int8_t i, j, k;
     double sum;
 
     for (i = 0; i < n; i++) {
       for (j = i; j < n; j++) {
-	            sum = a[i*n+j];
+        sum = a[i * n + j];
         for (k = i - 1; k >= 0; k--) {
-	                sum -= a[i*n+k] * a[j*n+k];
+          sum -= a[i * n + k] * a[j * n + k];
         }
         if (i == j) {
           if (sum <= 0) {
             return 1; /* error */
           }
           p[i] = sqrt(sum);
-	            }
+        } else {
-	            else {
+          a[j * n + i] = sum / p[i];
-	                a[j*n+i] = sum / p[i];
         }
       }
     }
@@ -46,52 +45,49 @@ private:
   }
 
   // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n)
+  static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
-	{
+    uint8_t i, j, k;
-		uint8_t i,j,k; double sum;
+    double sum;
     for (i = 0; i < n; i++)
-	        for (j = 0; j < n; j++)
+      for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
-	            a[i*n+j] = A[i*n+j];
     if (choldc1(a, p, n)) return 1;
     for (i = 0; i < n; i++) {
-	        a[i*n+i] = 1 / p[i];
+      a[i * n + i] = 1 / p[i];
       for (j = i + 1; j < n; j++) {
         sum = 0;
         for (k = i; k < j; k++) {
-	                sum -= a[j*n+k] * a[k*n+i];
+          sum -= a[j * n + k] * a[k * n + i];
         }
-	            a[j*n+i] = sum / p[j];
+        a[j * n + i] = sum / p[j];
       }
     }
 
     return 0; /* success */
   }
 
-
   // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	static uint8_t cholsl(double * A,double * a,double * p, uint8_t n)
+  static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
-	{
+    uint8_t i, j, k;
-		uint8_t i,j,k;
+    if (choldcsl(A, a, p, n)) return 1;
-	    if (choldcsl(A,a,p,n)) return 1;
     for (i = 0; i < n; i++) {
       for (j = i + 1; j < n; j++) {
-	            a[i*n+j] = 0.0;
+        a[i * n + j] = 0.0;
       }
     }
     for (i = 0; i < n; i++) {
-	        a[i*n+i] *= a[i*n+i];
+      a[i * n + i] *= a[i * n + i];
       for (k = i + 1; k < n; k++) {
-	            a[i*n+i] += a[k*n+i] * a[k*n+i];
+        a[i * n + i] += a[k * n + i] * a[k * n + i];
       }
       for (j = i + 1; j < n; j++) {
         for (k = j; k < n; k++) {
-	                a[i*n+j] += a[k*n+i] * a[k*n+j];
+          a[i * n + j] += a[k * n + i] * a[k * n + j];
         }
       }
     }
     for (i = 0; i < n; i++) {
       for (j = 0; j < i; j++) {
-	            a[i*n+j] = a[j*n+i];
+        a[i * n + j] = a[j * n + i];
       }
     }