From ee6b43aabfc10f6407cfe4650255648e20d73b61 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 28 Oct 2017 07:49:28 +1100 Subject: [PATCH] AP_NavEKF3: Fix bugs in use of wheel encoder data Found by pavloblindnology --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index ff9cb7d5cf..cc35d43614 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1579,10 +1579,10 @@ void NavEKF3_core::SelectBodyOdomFusion() } else if (storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms)) { // check if the delta time is too small to calculate a velocity - if (wheelOdmDataNew.delTime > EKF_TARGET_DT) { + if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) { // get the forward velocity - float fwdSpd = wheelOdmDataNew.delAng * wheelOdmDataNew.radius * (1.0f / wheelOdmDataNew.delTime); + float fwdSpd = wheelOdmDataDelayed.delAng * wheelOdmDataDelayed.radius * (1.0f / wheelOdmDataDelayed.delTime); // get the unit vector from the projection of the X axis onto the horizontal Vector3f unitVec; @@ -1598,7 +1598,7 @@ void NavEKF3_core::SelectBodyOdomFusion() // TODO write a dedicated observation model for wheel encoders usingWheelSensors = true; bodyOdmDataDelayed.vel = prevTnb * velNED; - bodyOdmDataDelayed.body_offset = wheelOdmDataNew.hub_offset; + bodyOdmDataDelayed.body_offset = wheelOdmDataDelayed.hub_offset; bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr; // Fuse data into the main filter