Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: ensure EK3 does not read GPS when disabled

This commit is contained in:
Andrew Tridgell 2024-05-30 09:04:57 +10:00
parent c7c95e086c
commit 41fce38b98
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -547,6 +547,11 @@ void NavEKF3_core::readGpsData()
// check for new GPS data
const auto &gps = dal.gps();
if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
// don't read GPS data when disabled in the current source set
return;
}
// limit update rate to avoid overflowing the FIFO buffer
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
return;

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -234,7 +234,8 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if(validOrigin) {
auto &gps = dal.gps();
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
if (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS
&& gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const Location &gpsloc = gps.location(selected_gps);
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
@ -348,7 +349,8 @@ bool NavEKF3_core::getLLH(Location &loc) const
bool NavEKF3_core::getGPSLLH(Location &loc) const
{
const auto &gps = dal.gps();
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D)) {
if (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS &&
gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
loc = gps.location(selected_gps);
return true;
}