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

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

This commit is contained in:
Pavlo Kolomiiets 2017-11-06 18:24:10 +02:00 committed by Randy Mackay
parent 753f360d06
commit 2c909cf83b
3 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/sensors.cpp
View File

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

6
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
View File

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

5
libraries/AP_WheelEncoder/AP_WheelEncoder.h
View File

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