From b4a8691a9ece76ba683faba84244c319a77d6f36 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 17 Oct 2019 20:02:18 +0900 Subject: [PATCH] AP_NavEKF3: remove wheel encoder update limit --- libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) 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; }