mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 17:23:34 +08:00
Use updated map_projection_XXX functions in apps
This commit is contained in:
@@ -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 */
|
||||
|
||||
Reference in New Issue
Block a user