diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-03 17:01:32 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-03 17:01:32 +0200 |
commit | cc11e8a0985f1a9790510fb3f19447a6c6946ba1 (patch) | |
tree | ec1a57fc67ff2ed92f3bd965ae66edf622dcf374 /src/modules | |
parent | 2b9909d97b446fbb112490d1631e70937beb80bf (diff) | |
download | px4-firmware-cc11e8a0985f1a9790510fb3f19447a6c6946ba1.tar.gz px4-firmware-cc11e8a0985f1a9790510fb3f19447a6c6946ba1.tar.bz2 px4-firmware-cc11e8a0985f1a9790510fb3f19447a6c6946ba1.zip |
EKF: Sync to upstream repo
Diffstat (limited to 'src/modules')
3 files changed, 32 insertions, 4 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 817590bab..967402fff 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -41,13 +41,17 @@ #include <string.h> #include <stdio.h> #include <stdarg.h> -#include <math.h> +#include <cmath> #include <algorithm> #ifndef M_PI_F #define M_PI_F static_cast<float>(M_PI) #endif +#ifndef isfinite +#define isfinite(__x) std::isfinite(__x) +#endif + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -2626,11 +2630,9 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } -#if 0 if (!isfinite(val)) { ekf_debug("constrain: non-finite!"); } -#endif return ret; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 83a738463..284a09902 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -114,6 +114,27 @@ Mat3f Mat3f::transpose() const return ret; } +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNEDr[0] = gpsGndSpd*cosf(gpsCourse); + velNEDr[1] = gpsGndSpd*sinf(gpsCourse); + velNEDr[2] = gpsVelD; +} + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) +{ + posNEDr[0] = earthRadius * (lat - latReference); + posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNEDr[2] = -(hgt - hgtReference); +} + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) +{ + lat = latRef + (double)posNEDi[0] * earthRadiusInv; + lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNEDi[2]; +} + // overload + operator to provide a vector addition Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index c137209ff..788fb85ec 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -45,7 +45,6 @@ #define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f -#define pi 3.141592657f #define earthRate 0.000072921f #define earthRadius 6378145.0 #define earthRadiusInv 1.5678540e-7 @@ -128,3 +127,9 @@ struct ekf_status_report { }; void ekf_debug(const char *fmt, ...); + +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference); + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); |