diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 12:08:28 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 12:08:28 +0200 |
commit | d93a64dd09a34515e4d9812408a530f3c9721a46 (patch) | |
tree | ee40670c03d9f484f73d2df0c9d13c6bbb86509a | |
parent | ccdfb19a4ac7c83643a013e3acf11ff745bd975f (diff) | |
download | px4-firmware-d93a64dd09a34515e4d9812408a530f3c9721a46.tar.gz px4-firmware-d93a64dd09a34515e4d9812408a530f3c9721a46.tar.bz2 px4-firmware-d93a64dd09a34515e4d9812408a530f3c9721a46.zip |
estimator: Geo conversions: Fix float / double types to prevent warnings. Fix calcLLH to actually return a value.
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 |
2 files changed, 8 insertions, 8 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 7a34c82f9..0894d60ae 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -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]; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 5a1f5125a..dc461cfa1 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -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]); |