diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 0fba1da29a..48cab13345 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1804,8 +1804,6 @@ void NavEKF3_core::SelectBodyOdomFusion() // start performance timer hal.util->perf_begin(_perf_FuseBodyOdom); - usingWheelSensors = false; - // Fuse data into the main filter FuseBodyVel(); @@ -1832,7 +1830,6 @@ void NavEKF3_core::SelectBodyOdomFusion() // This is a hack to enable use of the existing body frame velocity fusion method // TODO write a dedicated observation model for wheel encoders - usingWheelSensors = true; bodyOdmDataDelayed.vel = prevTnb * velNED; bodyOdmDataDelayed.body_offset = wheelOdmDataDelayed.hub_offset; bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index e507bbca05..aefe6dd5f1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -402,7 +402,6 @@ void NavEKF3_core::InitialiseVariables() bodyOdmMeasTime_ms = 0; bodyVelFusionDelayed = false; bodyVelFusionActive = false; - usingWheelSensors = false; wheelOdmMeasTime_ms = 0; // yaw sensor fusion diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c831b84955..12c740f475 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1261,7 +1261,6 @@ private: // wheel sensor fusion uint32_t wheelOdmMeasTime_ms; // time wheel odometry measurements were accepted for input to the data buffer (msec) - bool usingWheelSensors; // true when the body frame velocity fusion method should take onbservation data from the wheel odometry buffer obs_ring_buffer_t storedWheelOdm; // body velocity data buffer wheel_odm_elements wheelOdmDataNew; // Body frame odometry data at the current time horizon wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon