Use updated map_projection_XXX functions in apps

This commit is contained in:
Anton Babushkin
2014-03-17 22:20:41 +04:00
parent 2284a7e985
commit 3d5f52678f
4 changed files with 14 additions and 11 deletions
@@ -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;
@@ -49,6 +49,7 @@
#include <controllib/block/BlockParam.hpp>
#include <controllib/uorb/UOrbSubscription.hpp>
#include <controllib/uorb/UOrbPublication.hpp>
#include <lib/geo/geo.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -164,6 +165,7 @@ protected:
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
struct map_projection_reference_s ref; /**< local projection reference */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */