Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF3: ensure EK3 does not read GPS when disabled #27195

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+9 −2
Open

AP_NavEKF3: ensure EK3 does not read GPS when disabled #27195

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Copy link
Contributor

@rmackay9 rmackay9 May 30, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Although it's unusual a user could enable only altitude or only velocity (xy or z) while not using xy position so I think we need a more fine grained method to disable it. Also remember that even if the active source isn't GPS, it is still possible if EK3_SRC_OPTIONS = 1 (Fuse all velocities) that GPS velocities from another source set (e.g. not the active source set) could be used.

// 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;
Expand Down
6 changes: 4 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

One concern is that the position may jump as the user changes between source sets. So a source set that includes GPS will show the latest GPS position but if the user then switches back to external nav, it'll jump back to the last good position estimate.

I actually agree with this change or something like it. I've always thought it odd that the EKF3 sometimes reports the raw GPS position instead of the EKF position. We already report the raw GPS position to the GCS so I'm not sure we really need to report it again through the EKF. That's probably too big a change for the GCSs to absorb though.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I actually agree with this change or something like it. I've always thought it odd that the EKF3 sometimes reports the raw GPS position instead of the EKF position. We already report the raw GPS position to the GCS so I'm not sure we really need to report it again through the EKF. That's probably too big a change for the GCSs to absorb though.

Resolving that problem is complicated by the fact that the EKF can return a position estimate even when it is feeling ill. It will do this if it has ever managed to get an origin (i.e. be happy with where it is), and when the GPS is not capable of providing a location.

What that means is that never returning a GPS from the EKF would mean changing the interface we use onto the backends to be more complicated. e.g. "EK3 give me a location". What, you can't do that? OK, "GPS, what about you?" Really?! OK, "EK3, please give me your best guess". I don't think it will be all that pretty.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

... you can't really use the data if getLLH returns false; if it has never seen an origin and the GPS isn't giving a location then false really means that it hasn't given you anything at all. This is the sort of case you might end up flying to 0,0

&& 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();
Expand Down Expand Up @@ -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;
}
Expand Down
Loading