Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_WheelEncoder: 3d position offset

This commit is contained in:
Randy Mackay 2017-07-19 21:27:35 +09:00
parent f97e43919b
commit cd71984705
2 changed files with 44 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
View File

@ -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

5
libraries/AP_WheelEncoder/AP_WheelEncoder.h
View File

@ -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];