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

Merge pull request #1011 from PX4/mpc_in_flight_lock 

position_estimator_inav: handle in-flight GPS fix properly
This commit is contained in:
Lorenz Meier 2014-06-30 17:03:19 +02:00
parent 3c5f35da73 478f923331
commit 68c8090a39
1 changed files with 2 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -651,25 +651,22 @@ 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;
x_est[1] = gps.vel_n_m_s; x_est[1] = gps.vel_n_m_s;
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;
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", (double)lat, (double)lon, (double)alt); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
} }
} }