AP_WheelEncoder: 3d position offset

libraries/AP_WheelEncoder/AP_WheelEncoder.cpp

@@ -35,18 +35,26 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
     AP_GROUPINFO("_SCALING", 1, AP_WheelEncoder, _scaling[0], WHEELENCODER_SCALING_DEFAULT),
 
     // @Param: _POS_X
-    // @DisplayName: WheelEncoder X position
-    // @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
+    // @DisplayName: Wheel's X position offset
+    // @Description: X position of the center of the wheel in body frame. Positive X is forward of the origin.
     // @Units: m
-    // @User: Standard
-    AP_GROUPINFO("_POS_X",   2, AP_WheelEncoder, _pos_x[0], 0.0f),
+    // @Increment: 0.01
+    // @User: Advanced
 
     // @Param: _POS_Y
-    // @DisplayName: WheelEncoder Y position
-    // @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
+    // @DisplayName: Wheel's Y position offset
+    // @Description: Y position of the center of the wheel in body frame. Positive Y is to the right of the origin.
     // @Units: m
-    // @User: Standard
-    AP_GROUPINFO("_POS_Y",   3, AP_WheelEncoder, _pos_y[0], 0.0f),
+    // @Increment: 0.01
+    // @User: Advanced
+
+    // @Param: _POS_Z
+    // @DisplayName: Wheel's Z position offset
+    // @Description: Z position of the center of the wheel in body frame. Positive Z is down from the origin.
+    // @Units: m
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("_POS",     3, AP_WheelEncoder, _pos_offset[0], 0.0f),
 
     // @Param: _PINA
     // @DisplayName: Input Pin A
@@ -78,18 +86,26 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
     AP_GROUPINFO("2_SCALING",7, AP_WheelEncoder, _scaling[1], WHEELENCODER_SCALING_DEFAULT),
 
     // @Param: 2_POS_X
-    // @DisplayName: WheelEncoder X position
-    // @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
+    // @DisplayName: Wheel2's X position offset
+    // @Description: X position of the center of the second wheel in body frame. Positive X is forward of the origin.
     // @Units: m
-    // @User: Standard
-    AP_GROUPINFO("2_POS_X",  8, AP_WheelEncoder, _pos_x[1], 0.0f),
+    // @Increment: 0.01
+    // @User: Advanced
 
-    // @Param: _POS_Y
-    // @DisplayName: WheelEncoder Y position
-    // @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
+    // @Param: 2_POS_Y
+    // @DisplayName: Wheel2's Y position offset
+    // @Description: Y position of the center of the second wheel in body frame. Positive Y is to the right of the origin.
     // @Units: m
-    // @User: Standard
-    AP_GROUPINFO("2_POS_Y",  9, AP_WheelEncoder, _pos_y[1], 0.0f),
+    // @Increment: 0.01
+    // @User: Advanced
+
+    // @Param: 2_POS_Z
+    // @DisplayName: Wheel2's Z position offset
+    // @Description: Z position of the center of the second wheel in body frame. Positive Z is down from the origin.
+    // @Units: m
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("2_POS",    9, AP_WheelEncoder, _pos_offset[1], 0.0f),
 
     // @Param: 2_PINA
     // @DisplayName: Second Encoder Input Pin A
@@ -187,6 +203,15 @@ Vector2f AP_WheelEncoder::get_position(uint8_t instance) const
     return Vector2f(_pos_x[instance],_pos_y[instance]);
 }
 
+// get the total distance travelled in meters
+Vector3f AP_WheelEncoder::get_position(uint8_t instance) const
+{
+    // for invalid instances return zero vector
+    if (instance >= WHEELENCODER_MAX_INSTANCES) {
+        return Vector3f();
+    }
+    return _pos_offset[instance];
+}
 
 // get the total distance traveled in meters
 float AP_WheelEncoder::get_distance(uint8_t instance) const

libraries/AP_WheelEncoder/AP_WheelEncoder.h

@@ -65,7 +65,7 @@ public:
     bool enabled(uint8_t instance) const;
 
     // get the position of the wheel associated with the wheel encoder
-    Vector2f get_position(uint8_t instance) const;
+    Vector3f get_position(uint8_t instance) const;
 
     // get the total distance traveled in meters
     float get_distance(uint8_t instance) const;
@@ -88,8 +88,7 @@ protected:
     // parameters for each instance
     AP_Int8  _type[WHEELENCODER_MAX_INSTANCES];
     AP_Float _scaling[WHEELENCODER_MAX_INSTANCES];
-    AP_Float _pos_x[WHEELENCODER_MAX_INSTANCES];
-    AP_Float _pos_y[WHEELENCODER_MAX_INSTANCES];
+    AP_Vector3f _pos_offset[WHEELENCODER_MAX_INSTANCES];
     AP_Int8  _pina[WHEELENCODER_MAX_INSTANCES];
     AP_Int8  _pinb[WHEELENCODER_MAX_INSTANCES];