AttPosEKF: Fix coding style

2 files changed, 92 insertions, 92 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 6e654f496..d3f3fde7b 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -177,7 +177,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     float deltaQuat[4];
     const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);
 
-// Remove sensor bias errors
+    // Remove sensor bias errors
     correctedDelAng.x = dAngIMU.x - states[10];
     correctedDelAng.y = dAngIMU.y - states[11];
     correctedDelAng.z = dAngIMU.z - states[12];
@@ -192,14 +192,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     delAngTotal.y += correctedDelAng.y;
     delAngTotal.z += correctedDelAng.z;
 
-// Save current measurements
+    // Save current measurements
     Vector3f  prevDelAng = correctedDelAng;
 
-// Apply corrections for earths rotation rate and coning errors
-// * and + operators have been overloaded
+    // Apply corrections for earths rotation rate and coning errors
+    // * and + operators have been overloaded
     correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
 
-// Convert the rotation vector to its equivalent quaternion
+    // Convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
     if (rotationMag < 1e-12f)
     {
@@ -221,14 +221,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
         deltaQuat[3] = correctedDelAng.z*rotScaler;
     }
 
-// Update the quaternions by rotating from the previous attitude through
-// the delta angle rotation quaternion
+    // Update the quaternions by rotating from the previous attitude through
+    // the delta angle rotation quaternion
     qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
     qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
     qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
     qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
 
-// Normalise the quaternions and update the quaternion states
+    // Normalise the quaternions and update the quaternion states
     quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
     if (quatMag > 1e-16f)
     {
@@ -239,7 +239,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
         states[3] = quatMagInv*qUpdated[3];
     }
 
-// Calculate the body to nav cosine matrix
+    // Calculate the body to nav cosine matrix
     q00 = sq(states[0]);
     q11 = sq(states[1]);
     q22 = sq(states[2]);
@@ -263,29 +263,29 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 
     Tnb = Tbn.transpose();
 
-// transform body delta velocities to delta velocities in the nav frame
-// * and + operators have been overloaded
+    // transform body delta velocities to delta velocities in the nav frame
+    // * and + operators have been overloaded
     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
     delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
     delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
     delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
 
-// calculate the magnitude of the nav acceleration (required for GPS
-// variance estimation)
+    // calculate the magnitude of the nav acceleration (required for GPS
+    // variance estimation)
     accNavMag = delVelNav.length()/dtIMU;
 
-// If calculating position save previous velocity
+    // If calculating position save previous velocity
     float lastVelocity[3];
     lastVelocity[0] = states[4];
     lastVelocity[1] = states[5];
     lastVelocity[2] = states[6];
 
-// Sum delta velocities to get velocity
+    // Sum delta velocities to get velocity
     states[4] = states[4] + delVelNav.x;
     states[5] = states[5] + delVelNav.y;
     states[6] = states[6] + delVelNav.z;
 
-// If calculating postions, do a trapezoidal integration for position
+    // If calculating postions, do a trapezoidal integration for position
     states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
     states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
     states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
@@ -965,24 +965,24 @@ void AttPosEKF::updateDtVelPosFilt(float dt)
 void AttPosEKF::FuseVelposNED()
 {
 
-// declare variables used by fault isolation logic
+    // declare variables used by fault isolation logic
     uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
     uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
     uint32_t hgtRetryTime = 500; // height measurement retry time
     uint32_t horizRetryTime;
 
-// declare variables used to check measurement errors
+    // declare variables used to check measurement errors
     float velInnov[3] = {0.0f,0.0f,0.0f};
     float posInnov[2] = {0.0f,0.0f};
     float hgtInnov = 0.0f;
 
-// declare variables used to control access to arrays
+    // declare variables used to control access to arrays
     bool fuseData[6] = {false,false,false,false,false,false};
     uint8_t stateIndex;
     uint8_t obsIndex;
     uint8_t indexLimit = 21;
 
-// declare variables used by state and covariance update calculations
+    // declare variables used by state and covariance update calculations
     float velErr;
     float posErr;
     float R_OBS[6];
@@ -990,12 +990,12 @@ void AttPosEKF::FuseVelposNED()
     float SK;
     float quatMag;
 
-// Perform sequential fusion of GPS measurements. This assumes that the
-// errors in the different velocity and position components are
-// uncorrelated which is not true, however in the absence of covariance
-// data from the GPS receiver it is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
+    // Perform sequential fusion of GPS measurements. This assumes that the
+    // errors in the different velocity and position components are
+    // uncorrelated which is not true, however in the absence of covariance
+    // data from the GPS receiver it is the only assumption we can make
+    // so we might as well take advantage of the computational efficiencies
+    // associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData)
     {
         uint64_t tNow = getMicros();
@@ -1251,12 +1251,12 @@ void AttPosEKF::FuseMagnetometer()
         H_MAG[i] = 0.0f;
     }
 
-// Perform sequential fusion of Magnetometer measurements.
-// This assumes that the errors in the different components are
-// uncorrelated which is not true, however in the absence of covariance
-// data fit is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
+    // Perform sequential fusion of Magnetometer measurements.
+    // This assumes that the errors in the different components are
+    // uncorrelated which is not true, however in the absence of covariance
+    // data fit is the only assumption we can make
+    // so we might as well take advantage of the computational efficiencies
+    // associated with sequential fusion
     if (useCompass && fuseMagData && (obsIndex < 3))
     {
         // Calculate observation jacobians and Kalman gains
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index f8cd743cc..a4b95f14a 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -248,115 +248,115 @@ public:
     float minFlowRng;                       // minimum range over which to fuse optical flow measurements
     float moCompR_LOS;                      // scaler from sensor gyro rate to uncertainty in LOS rate
 
-void updateDtGpsFilt(float dt);
+    void updateDtGpsFilt(float dt);
 
-void updateDtHgtFilt(float dt);
+    void updateDtHgtFilt(float dt);
 
-void  UpdateStrapdownEquationsNED();
+    void  UpdateStrapdownEquationsNED();
 
-void CovariancePrediction(float dt);
+    void CovariancePrediction(float dt);
 
-void FuseVelposNED();
+    void FuseVelposNED();
 
-void FuseMagnetometer();
+    void FuseMagnetometer();
 
-void FuseAirspeed();
+    void FuseAirspeed();
 
-void FuseOptFlow();
+    void FuseOptFlow();
 
-void OpticalFlowEKF();
+    void OpticalFlowEKF();
 
-void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
+    void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
 
-void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
+    void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
 
-void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+    void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 
-// store staes along with system time stamp in msces
-void StoreStates(uint64_t timestamp_ms);
+    // store staes along with system time stamp in msces
+    void StoreStates(uint64_t timestamp_ms);
 
-/**
- * Recall the state vector.
- *
- * Recalls the vector stored at closest time to the one specified by msec
- *FuseOptFlow
- * @return zero on success, integer indicating the number of invalid states on failure.
- *         Does only copy valid states, if the statesForFusion vector was initialized
- *         correctly by the caller, the result can be safely used, but is a mixture
- *         time-wise where valid states were updated and invalid remained at the old
- *         value.
- */
-int RecallStates(float *statesForFusion, uint64_t msec);
+    /**
+     * Recall the state vector.
+     *
+     * Recalls the vector stored at closest time to the one specified by msec
+     *FuseOptFlow
+     * @return zero on success, integer indicating the number of invalid states on failure.
+     *         Does only copy valid states, if the statesForFusion vector was initialized
+     *         correctly by the caller, the result can be safely used, but is a mixture
+     *         time-wise where valid states were updated and invalid remained at the old
+     *         value.
+     */
+    int RecallStates(float *statesForFusion, uint64_t msec);
 
-void RecallOmega(float *omegaForFusion, uint64_t msec);
+    void RecallOmega(float *omegaForFusion, uint64_t msec);
 
-void ResetStoredStates();
+    void ResetStoredStates();
 
-void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
+    void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
 
-void calcEarthRateNED(Vector3f &omega, float latitude);
+    void calcEarthRateNED(Vector3f &omega, float latitude);
 
-static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+    static void eul2quat(float (&quat)[4], const float (&eul)[3]);
 
-static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+    static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
-static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+    static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+    void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
-static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
+    static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
 
-static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+    static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
-static float sq(float valIn);
+    static float sq(float valIn);
 
-static float maxf(float valIn1, float valIn2);
+    static float maxf(float valIn1, float valIn2);
 
-static float min(float valIn1, float valIn2);
+    static float min(float valIn1, float valIn2);
 
-void OnGroundCheck();
+    void OnGroundCheck();
 
-void CovarianceInit();
+    void CovarianceInit();
 
-void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+    void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
 
-float ConstrainFloat(float val, float min, float maxf);
+    float ConstrainFloat(float val, float min, float maxf);
 
-void ConstrainVariances();
+    void ConstrainVariances();
 
-void ConstrainStates();
+    void ConstrainStates();
 
-void ForceSymmetry();
+    void ForceSymmetry();
 
-int CheckAndBound(struct ekf_status_report *last_error);
+    int CheckAndBound(struct ekf_status_report *last_error);
 
-void ResetPosition();
+    void ResetPosition();
 
-void ResetVelocity();
+    void ResetVelocity();
 
-void ZeroVariables();
+    void ZeroVariables();
 
-void GetFilterState(struct ekf_status_report *state);
+    void GetFilterState(struct ekf_status_report *state);
 
-void GetLastErrorState(struct ekf_status_report *last_error);
+    void GetLastErrorState(struct ekf_status_report *last_error);
 
-bool StatesNaN();
+    bool StatesNaN();
 
-void InitializeDynamic(float (&initvelNED)[3], float declination);
+    void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
-void updateDtVelPosFilt(float dt);
+    void updateDtVelPosFilt(float dt);
 
-bool FilterHealthy();
+    bool FilterHealthy();
 
-bool GyroOffsetsDiverged();
+    bool GyroOffsetsDiverged();
 
-bool VelNEDDiverged();
+    bool VelNEDDiverged();
 
-void ResetHeight(void);
+    void ResetHeight(void);
 
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+    void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
 };