diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 17:03:19 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 17:03:19 +0200 |
commit | 68c8090a3948ac5c4ec7b98fc4e52e431e9bb95a (patch) | |
tree | 59242aaa2fd8df814317e96117648fc4598c50c3 | |
parent | 3c5f35da73cb6c37cf45a06909631e13b762f76d (diff) | |
parent | 478f923331cb8c3fd0e98a99afee462e4e158d44 (diff) | |
download | px4-firmware-68c8090a3948ac5c4ec7b98fc4e52e431e9bb95a.tar.gz px4-firmware-68c8090a3948ac5c4ec7b98fc4e52e431e9bb95a.tar.bz2 px4-firmware-68c8090a3948ac5c4ec7b98fc4e52e431e9bb95a.zip |
Merge pull request #1011 from PX4/mpc_in_flight_lock
position_estimator_inav: handle in-flight GPS fix properly
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 |
1 files changed, 2 insertions, 5 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5b312941e..db1ce79ca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -651,25 +651,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* update baro offset */ - baro_offset -= z_est[0]; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; - z_est[0] = 0.0f; local_pos.ref_lat = lat; local_pos.ref_lon = lon; - local_pos.ref_alt = alt; + local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); 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); } } |