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.
This commit is contained in:
Andrew Tridgell 2019-08-23 15:30:33 +10:00
parent fee3136b44
commit 414d3eb670

View File

@ -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();