From dd88fdf7e7706a395973c6fcea69bf093fc9967b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 May 2020 12:09:45 +1000 Subject: [PATCH] AP_GPS: fixed issue with GPS selection for moving baseline when you have a moving baseline pair of ublox GPS modules and the rover GPS does not have full fixed RTK lock on the base GPS then we should not use it as our primary GPS as it's position and velocity can be badly affected by the attempts of the GPS to gain a fixed lock. This was observed in a flight with two F9P GPS, where the GPS velocity data from the rover GPS went way off when it lost full RTK lock. It's status stayed at 4, so it was selected as the primary GPS --- libraries/AP_GPS/AP_GPS.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a82b4d249b..7cbb394b95 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -890,6 +890,29 @@ void AP_GPS::update_primary(void) } return; } + /* + if this is a BASE and the other GPS is a MB rover, then we + should force the use of the BASE GPS if the rover doesn't + have a yaw lock. This is important as when the rover doesn't + have a lock it will often report a higher status than the + base (eg. status=4), but the velocity and position data can + be very bad. As the rover is getting it's base position from + the base GPS then the position and velocity are expected to + be worse than the base GPS unless it has a full moving + baseline lock + */ + const uint8_t i2 = i^1; // the other GPS in the pair + if (_type[i] == GPS_TYPE_UBLOX_RTK_BASE && + state[i].status >= GPS_OK_FIX_3D && + _type[i2] == GPS_TYPE_UBLOX_RTK_ROVER && + (state[i2].status != GPS_OK_FIX_3D_RTK_FIXED || + !state[i2].have_gps_yaw)) { + if (primary_instance != i) { + _last_instance_swap_ms = now; + primary_instance = i; + } + return; + } } // handling switching away from blended GPS