diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index e1e8cd5881..9aae816c83 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -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); // 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); }