From 85082875906ffca937dbaaf3839188b17abd951e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Mar 2015 15:59:01 +0100 Subject: [PATCH] AttPosEKF: Remove unused code --- .../estimator_22states.cpp | 21 ------------------- .../estimator_22states.h | 7 ------- 2 files changed, 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index d5995f84da..c313e83af9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2516,27 +2516,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) -{ - posNED[0] = earthRadius * (lat - latReference); - posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); - posNED[2] = -(hgt - hgtReference); -} - -void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) -{ - lat = latRef + (double)posNED[0] * earthRadiusInv; - lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - void AttPosEKF::setOnGround(const bool isLanded) { _onGround = isLanded; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6d3076da79..9b23f4df44 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -288,7 +288,6 @@ public: * 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 @@ -307,12 +306,6 @@ public: static void quat2eul(float (&eul)[3], const float (&quat)[4]); - static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - - static 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 quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;}