Browse Source

AP_NavEKF3: fix using_external_yaw when using external nav

the first if would always return true meaning the method would always return false when using external nav
thanks to David Sastre for finding this!
zr-v5.1
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
commit
1814524fcb
  1. 10
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

10
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -537,16 +537,16 @@ bool NavEKF3_core::use_compass(void) const @@ -537,16 +537,16 @@ bool NavEKF3_core::use_compass(void) const
// are we using a yaw source other than the magnetomer?
bool NavEKF3_core::using_external_yaw(void) const
{
#if EK3_FEATURE_EXTERNAL_NAV
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
}
#if EK3_FEATURE_EXTERNAL_NAV
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
}
#endif
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
}
return false;
}

Loading…
Cancel
Save