From ce44326ba81ffa9afd1870c4244da62caa65056e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 2 May 2018 17:19:59 +0900 Subject: [PATCH] AR_AttitudeControl: fix get_steering_out_heading while reversing --- libraries/APM_Control/AR_AttitudeControl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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); }