Skip to content

Commit

Permalink
AP_NavEKF3: allow origin to be set even when using GPS
Browse files Browse the repository at this point in the history
not being able to set origin when using GPS is annoying.  We've adjusted the way we do positions in the EKF (local offsets) and generally (Vector3p), so even if the suer sets an origin a long way away it shouldn't hurt.
  • Loading branch information
peterbarker committed May 17, 2024
1 parent 61512de commit 79ff9d9
Showing 1 changed file with 4 additions and 6 deletions.
10 changes: 4 additions & 6 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1392,12 +1392,10 @@ bool NavEKF3::setOriginLLH(const Location &loc)
if (!core) {
return false;
}
if ((sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) || common_origin_valid) {
// we don't allow setting of the EKF origin if using GPS
// or if the EKF origin has already been set.
// This is to prevent accidental setting of EKF origin with an
// invalid position or height or causing upsets from a shifting origin.
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
if (common_origin_valid) {
// we don't allow resetting of the EKF origin.
// This is to prevent causing upsets from a shifting origin.
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3: origin already set");
return false;
}
bool ret = false;
Expand Down

0 comments on commit 79ff9d9

Please sign in to comment.