Browse Source

AP_NavEKF3: remove wheel encoder update limit

master
Randy Mackay 5 years ago
parent
commit
b4a8691a9e
  1. 1
      libraries/AP_NavEKF3/AP_NavEKF3.h
  2. 3
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

1
libraries/AP_NavEKF3/AP_NavEKF3.h

@ -233,6 +233,7 @@ public: @@ -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

@ -140,9 +140,10 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam @@ -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;
}

Loading…
Cancel
Save