fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

estimator: Geo conversions: Fix float / double types to prevent warnings. Fix calcLLH to actually return a value.

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-29 12:08:28 +02:00
parent ccdfb19a4a
commit d93a64dd09
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+7 -7
View File
@@ -1884,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd,
velNED[2] = gpsVelD;
}
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
{
posNED[0] = earthRadius * (lat - latRef);
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
posNED[2] = -(hgt - hgtRef);
posNED[0] = earthRadius * (lat - latReference);
posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
posNED[2] = -(hgt - hgtReference);
}
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + posNED[0] * earthRadiusInv;
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
lat = latRef + (double)posNED[0] * earthRadiusInv;
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
src/modules/ekf_att_pos_estimator/estimator_23states.h
+1 -1
View File
@@ -246,7 +246,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float 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]);