dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Remove unused code

This commit is contained in:
Johan Jansen 2015-03-13 15:59:01 +01:00
parent 4a8f799e9e
commit 8508287590
2 changed files with 0 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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;

7
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -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;}