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

AP_NavEKF3: Fix bugs in use of wheel encoder data

Browse Source 
Found by pavloblindnology
This commit is contained in:
priseborough 2017-10-28 07:49:28 +11:00 committed by Randy Mackay
parent 6d0cfc13a2
commit ee6b43aabf
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1579,10 +1579,10 @@ void NavEKF3_core::SelectBodyOdomFusion()
} else if (storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms)) { } else if (storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms)) {
// check if the delta time is too small to calculate a velocity // 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 // 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 // get the unit vector from the projection of the X axis onto the horizontal
Vector3f unitVec; Vector3f unitVec;
@ -1598,7 +1598,7 @@ void NavEKF3_core::SelectBodyOdomFusion()
// TODO write a dedicated observation model for wheel encoders // TODO write a dedicated observation model for wheel encoders
usingWheelSensors = true; usingWheelSensors = true;
bodyOdmDataDelayed.vel = prevTnb * velNED; bodyOdmDataDelayed.vel = prevTnb * velNED;
bodyOdmDataDelayed.body_offset = wheelOdmDataNew.hub_offset; bodyOdmDataDelayed.body_offset = wheelOdmDataDelayed.hub_offset;
bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr; bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr;
// Fuse data into the main filter // Fuse data into the main filter