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

AP_WheelEncoder: replace scaling with CPR and radius 

CPR is the wheel encoder's counts per revolution of the wheel
radius is the radius of the wheel in meters
This commit is contained in:
Randy Mackay 2017-07-19 21:28:45 +09:00
parent cd71984705
commit a53e254bd0
2 changed files with 52 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
View File

@ -27,12 +27,19 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
// @User: Standard
AP_GROUPINFO("_TYPE", 0, AP_WheelEncoder, _type[0], 0),
// @Param: _SCALING
// @DisplayName: WheelEncoder scaling
// @Description: Scaling factor between sensor reading and measured distance in millimeters
// @Param: _CPR
// @DisplayName: WheelEncoder counts per revolution
// @Description: WheelEncoder counts per full revolution of the wheel
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_CPR", 1, AP_WheelEncoder, _counts_per_revolution[0], WHEELENCODER_CPR_DEFAULT),
// @Param: _RADIUS
// @DisplayName: Wheel radius in meters
// @Description: Wheel radius in meters
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("_SCALING", 1, AP_WheelEncoder, _scaling[0], WHEELENCODER_SCALING_DEFAULT),
AP_GROUPINFO("_RADIUS", 2, AP_WheelEncoder, _wheel_radius[0], WHEELENCODER_RADIUS_DEFAULT),
// @Param: _POS_X
// @DisplayName: Wheel's X position offset
@ -78,12 +85,19 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
// @User: Standard
AP_GROUPINFO("2_TYPE", 6, AP_WheelEncoder, _type[1], 0),
// @Param: 2_SCALING
// @DisplayName: WheelEncoder scaling
// @Description: Scaling factor between sensor reading and measured distance in millimeters
// @Param: 2_CPR
// @DisplayName: WheelEncoder 2 counts per revolution
// @Description: WheelEncoder 2 counts per full revolution of the wheel
// @Increment: 1
// @User: Standard
AP_GROUPINFO("2_CPR", 7, AP_WheelEncoder, _counts_per_revolution[1], WHEELENCODER_CPR_DEFAULT),
// @Param: 2_RADIUS
// @DisplayName: Wheel2's radius in meters
// @Description: Wheel2's radius in meters
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("2_SCALING",7, AP_WheelEncoder, _scaling[1], WHEELENCODER_SCALING_DEFAULT),
AP_GROUPINFO("2_RADIUS", 8, AP_WheelEncoder, _wheel_radius[1], WHEELENCODER_RADIUS_DEFAULT),
// @Param: 2_POS_X
// @DisplayName: Wheel2's X position offset
@ -170,7 +184,7 @@ void AP_WheelEncoder::update(void)
}
}
}
// check if an instance is healthy
bool AP_WheelEncoder::healthy(uint8_t instance) const
{
@ -193,14 +207,24 @@ bool AP_WheelEncoder::enabled(uint8_t instance) const
return true;
}
// get the total distance travelled in meters
Vector2f AP_WheelEncoder::get_position(uint8_t instance) const
// get the counts per revolution of the encoder
uint16_t AP_WheelEncoder::get_counts_per_revolution(uint8_t instance) const
{
// for invalid instances return zero vector
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return Vector2f(0.0f, 0.0f);
return 0;
}
return Vector2f(_pos_x[instance],_pos_y[instance]);
return (uint16_t)_counts_per_revolution[instance];
}
// get the wheel radius in meters
float AP_WheelEncoder::get_wheel_radius(uint8_t instance) const
{
// for invalid instances return zero vector
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0.0f;
}
return _wheel_radius[instance];
}
// get the total distance travelled in meters
@ -220,7 +244,11 @@ float AP_WheelEncoder::get_distance(uint8_t instance) const
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0.0f;
}
return _scaling[instance] * state[instance].distance_count * 0.001f;
// protect against divide by zero
if (_counts_per_revolution[instance] == 0) {
return 0.0f;
}
return M_2PI * _wheel_radius[instance] * state[instance].distance_count / _counts_per_revolution[instance];
}
// get the total number of sensor reading from the encoder

12
libraries/AP_WheelEncoder/AP_WheelEncoder.h
View File

@ -21,7 +21,8 @@
// Maximum number of WheelEncoder measurement instances available on this platform
#define WHEELENCODER_MAX_INSTANCES 2
#define WHEELENCODER_SCALING_DEFAULT 0.05f // default scaling between sensor readings and millimeters
#define WHEELENCODER_CPR_DEFAULT 3200 // default encoder counts per full revolution of the wheel
#define WHEELENCODER_RADIUS_DEFAULT 0.05f // default wheel radius of 5cm (0.05m)
class AP_WheelEncoder_Backend;
@ -64,6 +65,12 @@ public:
// return true if the instance is enabled
bool enabled(uint8_t instance) const;
// get the counts per revolution of the encoder
uint16_t get_counts_per_revolution(uint8_t instance) const;
// 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;
@ -87,7 +94,8 @@ public:
protected:
// parameters for each instance
AP_Int8 _type[WHEELENCODER_MAX_INSTANCES];
AP_Float _scaling[WHEELENCODER_MAX_INSTANCES];
AP_Int16 _counts_per_revolution[WHEELENCODER_MAX_INSTANCES];
AP_Float _wheel_radius[WHEELENCODER_MAX_INSTANCES];
AP_Vector3f _pos_offset[WHEELENCODER_MAX_INSTANCES];
AP_Int8 _pina[WHEELENCODER_MAX_INSTANCES];
AP_Int8 _pinb[WHEELENCODER_MAX_INSTANCES];