AP_NavEKF2: fixed return of common origin

when common origin is set we should return that, not backends private
origin
This commit is contained in:
Andrew Tridgell 2024-08-21 14:40:10 +10:00
parent be5ecab686
commit cf45dbf284
1 changed files with 4 additions and 0 deletions

View File

@ -1099,6 +1099,10 @@ bool NavEKF2::getOriginLLH(Location &loc) const
if (!core) {
return false;
}
if (common_origin_valid) {
loc = common_EKF_origin;
return true;
}
return core[primary].getOriginLLH(loc);
}