dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -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;

2
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
View File

@ -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 */

5
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -132,6 +132,7 @@ static orb_advert_t telemetry_status_pub = -1;
static int32_t lat0 = 0;
static int32_t lon0 = 0;
static double alt0 = 0;
struct map_projection_reference_s hil_ref;
static void
handle_message(mavlink_message_t *msg)
@ -653,7 +654,7 @@ handle_message(mavlink_message_t *msg)
bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
double lat = hil_state.lat*1e-7;
double lon = hil_state.lon*1e-7;
map_projection_project(lat, lon, &x, &y);
map_projection_project(&hil_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@ -679,7 +680,7 @@ handle_message(mavlink_message_t *msg)
lat0 = hil_state.lat;
lon0 = hil_state.lon;
alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
map_projection_init(&hil_ref, hil_state.lat, hil_state.lon);
}
/* Calculate Rotation Matrix */

8
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -206,6 +206,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
struct map_projection_reference_s ref;
memset(&ref, 0, sizeof(ref));
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
@ -560,7 +562,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
@ -569,7 +571,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
@ -836,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.xy_global) {
double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.time_gps_usec = gps.time_gps_usec;