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

AP_NavEKF3: fixed output LLH to account for IMU offset

Browse Source 
thanks to Paul and Randy
This commit is contained in:
Andrew Tridgell 2021-08-09 18:02:14 +10:00
parent 72a927343a
commit ff59e0f72f
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -285,7 +285,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
// The EKF is able to provide a position estimate // The EKF is able to provide a position estimate
loc.lat = EKF_origin.lat; loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng; loc.lng = EKF_origin.lng;
loc.offset(outputDataNew.position.x, outputDataNew.position.y); loc.offset(outputDataNew.position.x + posOffsetNED.x,
outputDataNew.position.y + posOffsetNED.y);
return true; return true;
} else { } else {
// We have been be doing inertial dead reckoning for too long so use raw GPS if available // We have been be doing inertial dead reckoning for too long so use raw GPS if available
@ -295,7 +296,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
// Return the EKF estimate but mark it as invalid // Return the EKF estimate but mark it as invalid
loc.lat = EKF_origin.lat; loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng; loc.lng = EKF_origin.lng;
loc.offset(outputDataNew.position.x, outputDataNew.position.y); loc.offset(outputDataNew.position.x + posOffsetNED.x,
outputDataNew.position.y + posOffsetNED.y);
return false; return false;
} }
} }
@ -306,7 +308,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else { } else {
loc.lat = EKF_origin.lat; loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng; loc.lng = EKF_origin.lng;
loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y); loc.offset(lastKnownPositionNE.x + posOffsetNED.x,
lastKnownPositionNE.y + posOffsetNED.y);
return false; return false;
} }
} }