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

@@ -35,9 +35,9 @@ uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return
 
 ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                 LocalDataPoolManager &poolManager) {
-	  localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
+  localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
-	  localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
+  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;
+  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/CMakeLists.txt

@@ -3,4 +3,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
                                              AcsController.cpp)
 endif()
 
-add_subdirectory(acs)
+add_subdirectory(acs)

mission/controller/acs/CMakeLists.txt

@@ -1,13 +1,13 @@
 target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE AcsParameters.cpp
-  		  ActuatorCmd.cpp
+          ActuatorCmd.cpp
-  		  Guidance.cpp
+          Guidance.cpp
-  		  Igrf13Model.cpp
+          Igrf13Model.cpp
-  		  MultiplicativeKalmanFilter.cpp
+          MultiplicativeKalmanFilter.cpp
-  		  Navigation.cpp
+          Navigation.cpp
-  		  SensorProcessing.cpp
+          SensorProcessing.cpp
-  		  SensorValues.cpp
+          SensorValues.cpp
-  		  SusConverter.cpp)
+          SusConverter.cpp)
-  		  
+
 add_subdirectory(control)

mission/controller/acs/SusConverter.cpp

@@ -108,14 +108,14 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
 float* SusConverter::calculateSunVector() {
   // Calculate the normalized Sun Vector
   sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
-                           (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
+                              (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
-                                 powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
+                                    powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
   sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
-                           (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
+                              (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
-                                 powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
+                                    powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
   sunVectorSensorFrame[2] =
       -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
-                  powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
+                   powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
 
   return sunVectorSensorFrame;
 }

mission/controller/acs/SusConverter.h

@@ -27,10 +27,10 @@ class SusConverter {
                                  const float coeffBeta[9][10]);
 
  private:
-  float alphaBetaRaw[2];         //[°]
+  float alphaBetaRaw[2];          //[°]
-                                 //  float coeffAlpha[9][10];
+                                  //  float coeffAlpha[9][10];
-                                 //  float coeffBeta[9][10];
+                                  //  float coeffBeta[9][10];
-  float alphaBetaCalibrated[2];  //[°]
+  float alphaBetaCalibrated[2];   //[°]
   float sunVectorSensorFrame[3];  //[-]
 
   bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,

mission/controller/acs/config/classIds.h

@@ -5,15 +5,14 @@
 #include <fsfw/returnvalues/FwClassIds.h>
 
 namespace CLASS_ID {
-enum eiveclassIds: uint8_t {
+enum eiveclassIds : uint8_t {
-    EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
+  EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
-    KALMAN,
+  KALMAN,
-	SAFE,
+  SAFE,
-	PTG,
+  PTG,
-	DETUMBLE,
+  DETUMBLE,
-    EIVE_CLASS_ID_END // [EXPORT] : [END]
+  EIVE_CLASS_ID_END  // [EXPORT] : [END]
 };
 }
 
-
 #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}
+                                           SafeCtrl.cpp)
-  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_){
-
-	detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
-	magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
 
+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,38 +8,36 @@
 #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:
+  Detumble(AcsParameters *acsParameters_);
+  virtual ~Detumble();
 
-public:
+  static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
-	Detumble(AcsParameters *acsParameters_);
+  static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
-	virtual ~Detumble();
 
-	static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
+  /* @brief: Load AcsParameters für this class
-	static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
+   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
+   */
+  void loadAcsParameters(AcsParameters *acsParameters_);
 
-	/* @brief: Load AcsParameters für this class
+  ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
-	 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
+                        const bool magFieldValid, double *magMom);
-	 */
-	void loadAcsParameters(AcsParameters *acsParameters_);
 
-        ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
+  ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
-                              const double *magField, const bool magFieldValid, double *magMom);
 
-        ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
+ private:
-
+  AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
-       private:
+  AcsParameters::MagnetorquesParameter *magnetorquesParameter;
-	AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
-	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,100 +14,96 @@
 #include <fsfw/globalfunctions/sign.h>
 #include <math.h>
 
-PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
+#include "../util/MathOperations.h"
-	loadAcsParameters(acsParameters_);
+
+PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
+
+PtgCtrl::~PtgCtrl() {}
+
+void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
+  pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
+  inertiaEIVE = &(acsParameters_->inertiaEIVE);
+  rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
+  rwMatrices = &(acsParameters_->rwMatrices);
 }
 
-PtgCtrl::~PtgCtrl(){
+void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
+                               const double *rwPseudoInv, double *torqueRws) {
+  //------------------------------------------------------------------------------------------------
+  // Compute gain matrix K and P matrix
+  //------------------------------------------------------------------------------------------------
+  double om = pointingModeControllerParameters->om;
+  double zeta = pointingModeControllerParameters->zeta;
+  double qErrorMin = pointingModeControllerParameters->qiMin;
+  double omMax = pointingModeControllerParameters->omMax;
 
-}
+  double cInt = 2 * om * zeta;
+  double kInt = 2 * pow(om, 2);
 
-void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
+  double qErrorLaw[3] = {0, 0, 0};
-	pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
-	inertiaEIVE = &(acsParameters_->inertiaEIVE);
-	rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
-	rwMatrices =&(acsParameters_->rwMatrices);
-}
 
-void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
+  for (int i = 0; i < 3; i++) {
+    if (abs(qError[i]) < qErrorMin) {
+      qErrorLaw[i] = qErrorMin;
+    } else {
+      qErrorLaw[i] = abs(qError[i]);
+    }
+  }
+  double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
 
-	//------------------------------------------------------------------------------------------------
+  double gain1 = cInt * omMax / qErrorLawNorm;
-	// Compute gain matrix K and P matrix
+  double gainVector[3] = {0, 0, 0};
-	//------------------------------------------------------------------------------------------------
+  VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
-	double om = pointingModeControllerParameters->om;
-	double zeta = pointingModeControllerParameters->zeta;
-	double qErrorMin = pointingModeControllerParameters->qiMin;
-	double omMax = pointingModeControllerParameters->omMax;
 
-	double cInt = 2 * om * zeta;
+  double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-	double kInt = 2 * pow(om,2);
+  double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  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);
 
-	double qErrorLaw[3] = {0, 0, 0};
+  // Inverse of gainMatrix
+  double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
+  gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
+  gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
 
-	for (int i = 0; i < 3; i++) {
+  double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-		if (abs(qError[i]) < qErrorMin) {
+  MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
-			qErrorLaw[i] = qErrorMin;
+                                     3, 3);
-		}
+  MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
-		else {
-			qErrorLaw[i] = abs(qError[i]);
-		}
-	}
-	double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
 
-	double gain1 = cInt * omMax / qErrorLawNorm;
+  //------------------------------------------------------------------------------------------------
-	double gainVector[3] = {0, 0, 0};
+  // Torque Calculations for the reaction wheels
-	VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
+  //------------------------------------------------------------------------------------------------
 
-	double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double pError[3] = {0, 0, 0};
-	double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
-	gainMatrixDiagonal[0][0] =  gainVector[0];
+  double pErrorSign[3] = {0, 0, 0};
-	gainMatrixDiagonal[1][1] =  gainVector[1];
-	gainMatrixDiagonal[2][2] =  gainVector[2];
-	MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
 
-	// Inverse of gainMatrix
+  for (int i = 0; i < 3; i++) {
-	double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    if (abs(pError[i]) > 1) {
-	gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
+      pErrorSign[i] = sign(pError[i]);
-	gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
+    } else {
-	gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
+      pErrorSign[i] = pError[i];
+    }
+  }
+  //	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);
 
-	double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  //  Torque for rate error
-	MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
+  double torqueRate[3] = {0, 0, 0};
-	MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
+  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
-
+  VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
-	//------------------------------------------------------------------------------------------------
+  VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
-	// Torque Calculations for the reaction wheels
-	//------------------------------------------------------------------------------------------------
-
-	double pError[3] = {0, 0, 0};
-	MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
-	double pErrorSign[3] = {0, 0, 0};
-
-	for (int i = 0; i < 3; i++) {
-
-		if (abs(pError[i]) > 1) {
-			pErrorSign[i] = sign(pError[i]);
-		}
-		else {
-			pErrorSign[i] = pError[i];
-		}
-	}
-//	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
-	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
-	double torque[3] = {0, 0, 0};
-	VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
-	MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
 
+  //	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,95 +8,91 @@
 #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
-		// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+    return cholsl(matrix, result, tempMatrix, dimension);
-		return cholsl(matrix, result, tempMatrix, dimension);
+  }
-	}
-private:
-	// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	static uint8_t choldc1(double * a, double * p, uint8_t n) {
-		int8_t i,j,k;
-	    double sum;
 
-	    for (i = 0; i < n; i++) {
+ private:
-	        for (j = i; j < n; j++) {
+  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	            sum = a[i*n+j];
+  static uint8_t choldc1(double *a, double *p, uint8_t n) {
-	            for (k = i - 1; k >= 0; k--) {
+    int8_t i, j, k;
-	                sum -= a[i*n+k] * a[j*n+k];
+    double sum;
-	            }
-	            if (i == j) {
-	                if (sum <= 0) {
-	                    return 1; /* error */
-	                }
-	                p[i] = sqrt(sum);
-	            }
-	            else {
-	                a[j*n+i] = sum / p[i];
-	            }
-	        }
-	    }
 
-	    return 0; /* success */
+    for (i = 0; i < n; i++) {
-	}
+      for (j = i; j < n; j++) {
+        sum = a[i * n + j];
+        for (k = i - 1; k >= 0; k--) {
+          sum -= a[i * n + k] * a[j * n + k];
+        }
+        if (i == j) {
+          if (sum <= 0) {
+            return 1; /* error */
+          }
+          p[i] = sqrt(sum);
+        } else {
+          a[j * n + i] = sum / p[i];
+        }
+      }
+    }
 
-	// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+    return 0; /* success */
-	static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n)
+  }
-	{
-		uint8_t i,j,k; double sum;
-	    for (i = 0; i < n; i++)
-	        for (j = 0; j < 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];
-	        for (j = i + 1; j < n; j++) {
-	            sum = 0;
-	            for (k = i; k < j; k++) {
-	                sum -= a[j*n+k] * a[k*n+i];
-	            }
-	            a[j*n+i] = sum / p[j];
-	        }
-	    }
 
-	    return 0; /* success */
+  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-	}
+  static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
+    uint8_t i, j, k;
+    double sum;
+    for (i = 0; i < n; i++)
+      for (j = 0; j < 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];
+      for (j = i + 1; j < n; j++) {
+        sum = 0;
+        for (k = i; k < j; k++) {
+          sum -= a[j * n + k] * a[k * n + i];
+        }
+        a[j * n + i] = sum / p[j];
+      }
+    }
 
+    return 0; /* success */
+  }
 
-	// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+  // 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 (i = 0; i < n; i++) {
+      for (j = i + 1; j < n; j++) {
-	        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++) {
-	    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++) {
-	        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 (j = i + 1; j < n; j++) {
+        for (k = j; k < n; k++) {
-	            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 (i = 0; i < n; i++) {
+      for (j = 0; j < i; j++) {
-	        for (j = 0; j < i; j++) {
+        a[i * n + j] = a[j * n + i];
-	            a[i*n+j] = a[j*n+i];
+      }
-	        }
+    }
-	    }
 
-	    return 0; /* success */
+    return 0; /* success */
-	}
+  }
 };
 
 #endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */