fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 03:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_estimator_inav: don't change local z on first time ref initialization

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-27 14:52:42 +02:00
parent 36495cdb62
commit 16ca6c1605
1 changed files with 2 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+2 -5
View File
@@ -622,8 +622,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) { } else if (t > ref_init_start + ref_init_delay) {
ref_inited = true; ref_inited = true;
/* update baro offset */
baro_offset -= z_est[0];
/* set position estimate to (0, 0, 0), use GPS velocity for XY */ /* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f; x_est[0] = 0.0f;
@@ -631,18 +629,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
x_est[2] = accel_NED[0]; x_est[2] = accel_NED[0];
y_est[0] = 0.0f; y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s; y_est[1] = gps.vel_e_m_s;
z_est[0] = 0.0f;
y_est[2] = accel_NED[1]; y_est[2] = accel_NED[1];
local_pos.ref_lat = lat; local_pos.ref_lat = lat;
local_pos.ref_lon = lon; local_pos.ref_lon = lon;
local_pos.ref_alt = alt; local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t; local_pos.ref_timestamp = t;
/* initialize projection */ /* initialize projection */
map_projection_init(&ref, lat, lon); map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); 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); mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", lat, lon, alt);
} }
} }