Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: remove wheel encoder update limit

Browse Source
This commit is contained in:
Randy Mackay 2019-10-17 20:02:18 +09:00
parent 93aa600e5d
commit b4a8691a9e
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) * timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m) * posOffset is the XYZ body frame position of the wheel hub (m)
* radius is the effective rolling radius of the wheel (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); 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. // It uses the exisiting body frame velocity fusion.
// TODO implement a dedicated wheel odometry observation model // 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 // 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 // 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; return;
} }