diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-27 14:52:42 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-27 14:52:42 +0200 |
| commit | 16ca6c1605e0864d37cce4cfdbe790df5746bb38 (patch) | |
| tree | be6839abf235722ec4e2dabe8c3eb6f66b84601e /src/modules/position_estimator_inav | |
| parent | 36495cdb62e21b30a5a1851ec802c9f6a40c1171 (diff) | |
| download | px4-firmware-16ca6c1605e0864d37cce4cfdbe790df5746bb38.tar.gz px4-firmware-16ca6c1605e0864d37cce4cfdbe790df5746bb38.tar.bz2 px4-firmware-16ca6c1605e0864d37cce4cfdbe790df5746bb38.zip | |
position_estimator_inav: don't change local z on first time ref initialization
Diffstat (limited to 'src/modules/position_estimator_inav')
| -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 fdc3233e0..f236d546b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -622,8 +622,6 @@ 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; @@ -631,18 +629,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; - z_est[0] = 0.0f; y_est[2] = accel_NED[1]; 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", 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); } } |