diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index ea43d77259..2bb52b9148 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -237,8 +237,8 @@ Vector3f AP_WheelEncoder::get_position(uint8_t instance) const return _pos_offset[instance]; } -// get the total distance traveled in meters -float AP_WheelEncoder::get_distance(uint8_t instance) const +// get total delta angle (in radians) measured by the wheel encoder +float AP_WheelEncoder::get_delta_angle(uint8_t instance) const { // for invalid instances return zero if (instance >= WHEELENCODER_MAX_INSTANCES) { @@ -248,7 +248,14 @@ float AP_WheelEncoder::get_distance(uint8_t instance) const if (_counts_per_revolution[instance] == 0) { return 0.0f; } - return M_2PI * _wheel_radius[instance] * state[instance].distance_count / _counts_per_revolution[instance]; + return M_2PI * state[instance].distance_count / _counts_per_revolution[instance]; +} + +// get the total distance traveled in meters +float AP_WheelEncoder::get_distance(uint8_t instance) const +{ + // for invalid instances return zero + return get_delta_angle(instance) * _wheel_radius[instance]; } // get the total number of sensor reading from the encoder diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.h b/libraries/AP_WheelEncoder/AP_WheelEncoder.h index b9ed303064..4f4d85b1e9 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.h +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.h @@ -74,7 +74,10 @@ public: // get the position of the wheel associated with the wheel encoder Vector3f get_position(uint8_t instance) const; - // get the total distance traveled in meters + // get total delta angle (in radians) measured by the wheel encoder + float get_delta_angle(uint8_t instance) const; + + // get the total distance traveled in meters or radians float get_distance(uint8_t instance) const; // get the total number of sensor reading from the encoder