diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-17 22:20:41 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-17 22:20:41 +0400 |
| commit | 3d5f52678fa093a248d824828fcafe12ac2f8f15 (patch) | |
| tree | b95401fd45205dac6c8311de68ad0cdb101364a2 /src/modules/att_pos_estimator_ekf/KalmanNav.cpp | |
| parent | 2284a7e985b174dab4b3c1666d9f019d9479a230 (diff) | |
| download | px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.tar.gz px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.tar.bz2 px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.zip | |
Use updated map_projection_XXX functions in apps
Diffstat (limited to 'src/modules/att_pos_estimator_ekf/KalmanNav.cpp')
| -rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 10 |
1 files changed, 4 insertions, 6 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 668bac5d9..a4d5560c7 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -97,6 +97,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + memset(&ref, 0, sizeof(ref)); + F.zero(); G.zero(); V.zero(); @@ -247,11 +249,7 @@ void KalmanNav::update() lat0 = lat; lon0 = lon; alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); + map_projection_init(&ref, lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", @@ -327,7 +325,7 @@ void KalmanNav::updatePublications() float x; float y; bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); + map_projection_project(&ref, lat, lon, &x, &y); _localPos.timestamp = _pubTimeStamp; _localPos.xy_valid = true; _localPos.z_valid = true; |