Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: remove wheel encoder update limit

This commit is contained in:
Randy Mackay 2019-10-17 20:02:18 +09:00 committed by Andrew Tridgell
parent b58605a3b0
commit 931fb2f986
2 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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);

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;
}