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

position_estimator_inav, mc_pos_control: precise position reprojection on home position changes

This commit is contained in:
Anton Babushkin 2014-04-05 18:11:51 +04:00
parent 0fd6fb53f3
commit 79d2247b44
2 changed files with 41 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -473,18 +473,26 @@ void
MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
if (_ref_timestamp != 0) {
/* reproject local position setpoint to new reference */
float dx, dy;
map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy);
_pos_sp(0) -= dx;
_pos_sp(1) -= dy;
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos.ref_timestamp;
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
}
}

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

@ -543,25 +543,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (home.timestamp != home_timestamp) {
home_timestamp = home.timestamp;
if (ref_inited) {
/* reproject position estimate to new reference */
float dx, dy;
map_projection_project(&ref, home.lat, home.lon, &dx, &dy);
x_est[0] -= dx;
y_est[0] -= dy;
z_est[0] += home.alt - local_pos.ref_alt;
}
/* update baro offset */
baro_offset -= home.alt - local_pos.ref_alt;
double est_lat, est_lon;
float est_alt;
if (ref_inited) {
/* calculate current estimated position in global frame */
est_alt = local_pos.ref_alt - local_pos.z;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
}
/* update reference */
map_projection_init(&ref, home.lat, home.lon);
/* update baro offset */
baro_offset += home.alt - local_pos.ref_alt;
local_pos.ref_lat = home.lat;
local_pos.ref_lon = home.lon;
local_pos.ref_alt = home.alt;
local_pos.ref_timestamp = home.timestamp;
if (ref_inited) {
/* reproject position estimate with new reference */
map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
z_est[0] = -(est_alt - local_pos.ref_alt);
}
ref_inited = true;
}
}
@ -589,6 +598,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (gps_valid) {
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
@ -596,11 +609,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* update baro offset */
baro_offset -= z_est[0];
@ -628,7 +636,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(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
/* reset position estimate when GPS becomes good */
if (reset_est) {
@ -643,7 +651,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {