forked from Archive/PX4-Autopilot
AttPosEKF: Remove unused code
This commit is contained in:
parent
4a8f799e9e
commit
8508287590
|
@ -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]));
|
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)
|
void AttPosEKF::setOnGround(const bool isLanded)
|
||||||
{
|
{
|
||||||
_onGround = isLanded;
|
_onGround = isLanded;
|
||||||
|
|
|
@ -288,7 +288,6 @@ public:
|
||||||
* Recall the state vector.
|
* Recall the state vector.
|
||||||
*
|
*
|
||||||
* Recalls the vector stored at closest time to the one specified by msec
|
* 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.
|
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
* 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
|
* 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 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 void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||||
|
|
||||||
static inline float sq(float valIn) {return valIn * valIn;}
|
static inline float sq(float valIn) {return valIn * valIn;}
|
||||||
|
|
Loading…
Reference in New Issue