From 2c909cf83bf486ce28f8955228e39e752e2f4bcc Mon Sep 17 00:00:00 2001 From: Pavlo Kolomiiets Date: Mon, 6 Nov 2017 18:24:10 +0200 Subject: [PATCH] AP_WheelEncoder: Wheel encoder's offsets are properly propagated to EKF3. --- APMrover2/sensors.cpp | 2 +- libraries/AP_WheelEncoder/AP_WheelEncoder.cpp | 6 +++--- libraries/AP_WheelEncoder/AP_WheelEncoder.h | 5 +++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 8647b84e44..abc38be44a 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -119,7 +119,7 @@ void Rover::update_wheel_encoder() * timeStamp_ms is the time when the rotation was last measured (msec) * posOffset is the XYZ body frame position of the wheel hub (m) */ - EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_position(i), g2.wheel_encoder.get_wheel_radius(i)); + EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_pos_offset(i), g2.wheel_encoder.get_wheel_radius(i)); // calculate rpm for reporting to GCS if (is_positive(delta_time)) { diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index e3f96a15e9..bc73b20102 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -224,12 +224,12 @@ float AP_WheelEncoder::get_wheel_radius(uint8_t instance) const return _wheel_radius[instance]; } -// get the total distance travelled in meters -Vector3f AP_WheelEncoder::get_position(uint8_t instance) const +// return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin +const Vector3f &AP_WheelEncoder::get_pos_offset(uint8_t instance) const { // for invalid instances return zero vector if (instance >= WHEELENCODER_MAX_INSTANCES) { - return Vector3f(); + return pos_offset_zero; } return _pos_offset[instance]; } diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.h b/libraries/AP_WheelEncoder/AP_WheelEncoder.h index c07d3bc3b5..7102bba808 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.h +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.h @@ -77,8 +77,8 @@ public: // get the wheel radius in meters float get_wheel_radius(uint8_t instance) const; - // get the position of the wheel associated with the wheel encoder - Vector3f get_position(uint8_t instance) const; + // return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin + const Vector3f &get_pos_offset(uint8_t instance) const; // get total delta angle (in radians) measured by the wheel encoder float get_delta_angle(uint8_t instance) const; @@ -115,4 +115,5 @@ protected: WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES]; AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES]; uint8_t num_instances; + Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests };