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

AR_AttitudeControl: fix get_steering_out_heading while reversing

Browse Source
This commit is contained in:
Randy Mackay 2018-05-02 17:19:59 +09:00
parent 341d75e0c9
commit ce44326ba8
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -213,7 +213,10 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
// Calculate the desired turn rate (in radians) from the angle error (also in radians) // Calculate the desired turn rate (in radians) from the angle error (also in radians)
const float desired_rate = _steer_angle_p.get_p(yaw_error); float desired_rate = _steer_angle_p.get_p(yaw_error);
if (reversed) {
desired_rate = -desired_rate;
}
return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, reversed); return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, reversed);
} }