diff options
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 6 | ||||
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.h | 4 |
2 files changed, 6 insertions, 4 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index adf6961ef..4e4ef3b92 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -6,10 +6,10 @@ #include <algorithm> #ifndef M_PI_F -#define M_PI_F ((float)M_PI) +#define M_PI_F static_cast<float>(M_PI) #endif -#define EKF_COVARIANCE_DIVERGED 1.0e8f +constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), @@ -2401,6 +2401,7 @@ void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec) } } +#if 0 void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2425,6 +2426,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.x.z = 2*(q13 - q02); Tnb.y.z = 2*(q23 + q01); } +#endif void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index de21a75ca..898105db4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -218,7 +218,7 @@ public: void updateDtHgtFilt(float dt); - void UpdateStrapdownEquationsNED(); + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -272,7 +272,7 @@ public: 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 inline float sq(float valIn) {return valIn * valIn;} |