From 7787bb4fd50280e7f329094c5fe6fcb29f0da797 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Wed, 17 Oct 2012 10:18:24 +0900
Subject: [PATCH] ArduCopter: bug fix for get_yaw_rate_stabilized_ef

Provided by Leonard Hall
---
 ArduCopter/Attitude.pde | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index b44503c88..1560d6229 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -157,22 +157,22 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
     // convert the input to the desired yaw rate
     int32_t target_rate = stick_angle * g.acro_p;
 
-    // convert the input to the desired yaw rate
+    // update current target heading using pilot's desired rate of turn
     nav_yaw += target_rate * G_Dt;
     nav_yaw = wrap_360(nav_yaw);
 
-    // angle error with maximum of +- max_angle_overshoot
+    // calculate difference between desired heading and current heading
     angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor);
 
-    // this limits the maximum overshoot
+    // limit the maximum overshoot
     angle_error	= constrain(angle_error, -MAX_ANGLE_OVERSHOOT, MAX_ANGLE_OVERSHOOT);
 
     if (motors.armed() == false || ((g.rc_3.control_in == 0) && !failsafe)) {
     	angle_error = 0;
     }
 
-    // update nav_yaw to be within max_angle_overshoot of our current heading
-    nav_yaw = angle_error + ahrs.yaw_sensor;
+    // set nav_yaw to our desired heading
+    nav_yaw = wrap_360(angle_error + ahrs.yaw_sensor);
 
     // set earth frame targets for rate controller
 	set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
-- 
GitLab