AP_WheelEncoder: Wheel encoder's offsets are properly propagated to EKF3.

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)) {

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
+// return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin
-Vector3f AP_WheelEncoder::get_position(uint8_t instance) const
+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];
 }

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
+    // return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin
-    Vector3f get_position(uint8_t instance) const;
+    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
 };