Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues 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
loc.lat = EKF_origin.lat;
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;
} else {
// 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
loc.lat = EKF_origin.lat;
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;
}
}
@ -306,7 +308,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else {
loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng;
loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y);
loc.offset(lastKnownPositionNE.x + posOffsetNED.x,
lastKnownPositionNE.y + posOffsetNED.y);
return false;
}
}