From 2c7b9d9cf701ef794fa0fc5b244a1ba7bd93c89c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 20 Jul 2017 15:11:45 +0900 Subject: [PATCH] AP_WheelEncoder: add get_delta_angle The total angular change measured by the wheel encoder in radians --- libraries/AP_WheelEncoder/AP_WheelEncoder.cpp | 13 ++++++++++--- libraries/AP_WheelEncoder/AP_WheelEncoder.h | 5 ++++- 2 files changed, 14 insertions(+), 4 deletions(-) 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