From a10a228092a1c7131407a8adb679158ae4c7a908 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 2 Mar 2020 16:15:54 +0100 Subject: [PATCH] EKF2 GPS blending: reset relative position offsets if blending not feasible Signed-off-by: CarlOlsson --- src/modules/ekf2/ekf2_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 26fa60f001..6902b81249 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -973,6 +973,11 @@ void Ekf2::Run() // Only use selected receiver data if it has been updated _gps_new_output_data = (gps1_updated && _gps_select_index == 0) || (gps2_updated && _gps_select_index == 1); + + // Reset relative position offsets to zero + _NE_pos_offset_m[0].zero(); + _NE_pos_offset_m[1].zero(); + _hgt_offset_mm[0] = _hgt_offset_mm[1] = 0.0f; } if (_gps_new_output_data) {