Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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) * timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m) * 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 // calculate rpm for reporting to GCS
if (is_positive(delta_time)) { 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]; 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 // for invalid instances return zero vector
if (instance >= WHEELENCODER_MAX_INSTANCES) { if (instance >= WHEELENCODER_MAX_INSTANCES) {
return Vector3f(); return pos_offset_zero;
} }
return _pos_offset[instance]; return _pos_offset[instance];
} }

5
libraries/AP_WheelEncoder/AP_WheelEncoder.h
View File

@ -77,8 +77,8 @@ public:
// get the wheel radius in meters // get the wheel radius in meters
float get_wheel_radius(uint8_t instance) const; 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 // get total delta angle (in radians) measured by the wheel encoder
float get_delta_angle(uint8_t instance) const; float get_delta_angle(uint8_t instance) const;
@ -115,4 +115,5 @@ protected:
WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES]; WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES];
AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES]; AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES];
uint8_t num_instances; uint8_t num_instances;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
}; };