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: body and wheel odometry check source

Browse Source
This commit is contained in:
Randy Mackay 2020-08-17 21:04:31 +09:00
parent 1263648d88
commit 2505fd5c1c
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1782,13 +1782,17 @@ void NavEKF3_core::SelectBodyOdomFusion()
bodyVelFusionDelayed = false;
}
// Check for data at the fusion time horizon
if (storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms)) {
// Check for body odometry data (aka visual position delta) at the fusion time horizon
const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms);
if (bodyOdomDataToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) {
// Fuse data into the main filter
FuseBodyVel();
}
} else if (storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms)) {
// Check for wheel encoder data at the fusion time horizon
const bool wheelOdomDataToFuse = storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms);
if (wheelOdomDataToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) {
// check if the delta time is too small to calculate a velocity
if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) {