From 414d3eb670e244922c1cedef41d20ccc17b3fcc0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 23 Aug 2019 15:30:33 +1000 Subject: [PATCH] AP_NavEKF2: don't fuse GPS when EK2_GPS_TYPE=3 when using a vision position system, the user may have vision derived GPS data coming in using GPS_INPUT msgs. We should not fuse these when EK2_GPS_TYPE=3 as we end up fusing both vision data and GPS data, which does not work with the current EK2 code This change makes it possible to run EK2 and EK3 in parallel in a Vicon, wityh EK2 using VISION_POSITION_ESTIMATE data and EK3 using GPS_INPUT (with yaw) data. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index e4646c0e5a..76cbaf1441 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -463,6 +463,11 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d // check for new valid GPS data and update stored measurement if available void NavEKF2_core::readGpsData() { + if (frontend->_fusionModeGPS == 3) { + // don't read GPS data if GPS usage disabled + return; + } + // check for new GPS data // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer const AP_GPS &gps = AP::gps();