aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: accept set origin even when using GPS

Browse Source
This commit is contained in:
Randy Mackay 2023-11-28 15:03:33 +09:00 committed by Andrew Tridgell
parent ddae068657
commit 4e2dd7c399
3 changed files with 5 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1392,11 +1392,9 @@ 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.
if (common_origin_valid) {
// we don't allow setting the EKF origin if it has already been set
// this is to prevent causing upsets from a shifting origin.
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
return false;
}

7
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -652,14 +652,9 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
}
// sets the local NED origin using a LLH location (latitude, longitude, height)
// returns false if absolute aiding and GPS is being used or if the origin is already set
// returns false if the origin is already set
bool NavEKF3_core::setOriginLLH(const Location &loc)
{
if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
// reject attempts to set the origin if GPS is being used or if the origin is already set
return false;
}
return setOrigin(loc);
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -224,7 +224,7 @@ public:
// set the latitude and longitude and height used to set the NED origin
// All NED positions calculated by the filter will be relative to this location
// returns false if Absolute aiding and GPS is being used or if the origin is already set
// returns false if the origin has already been set
bool setOriginLLH(const Location &loc);
// Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty