From 3ee68987100ff0d412c8eef20e91b986a085829d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 29 Jul 2017 15:31:34 +1000 Subject: [PATCH] EKF: Enable origin to be maintained when starting aiding using EV only When starting aiding using EV only and commencing GPS aiding later, this change means that the GPS origin is set to the local position 0,0 point rather than the current vehicle position. This avoids large changes in local position when GPs aiding starts. --- EKF/gps_checks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index b03f72d0f7..6f7b8afaf7 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -68,7 +68,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating - if (_control_status.flags.opt_flow || _control_status.flags.gps) { + if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);