diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 35de0716f9..7b54b55d3c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -233,6 +233,7 @@ public: * timeStamp_ms is the time when the rotation was last measured (msec) * posOffset is the XYZ body frame position of the wheel hub (m) * radius is the effective rolling radius of the wheel (m) + * this should not be called at more than the EKF's update rate (50hz or 100hz) */ void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 17bdcff00b..f77cf3ca4c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -140,9 +140,10 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam // It uses the exisiting body frame velocity fusion. // TODO implement a dedicated wheel odometry observation model + // rate limiting to 50hz should be done by the caller // limit update rate to maximum allowed by sensor buffers and fusion process // don't try to write to buffer until the filter has been initialised - if (((timeStamp_ms - wheelOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) { + if ((delTime < dtEkfAvg) || !statesInitialised) { return; }