From 1b789b792806072fce3ca928b69d8da96b721a1f Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 7 Jun 2012 17:15:33 -0400 Subject: [PATCH] Yaw Fix Modified Nav_Yaw controller to better track intended heading changes. --- ArduCopter/Attitude.pde | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 245de6fc94..d3302227ef 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -254,8 +254,12 @@ get_rate_yaw(int32_t target_rate) output = p+i+d; // output control: + #if FRAME_CONFIG == HELI_FRAME + int16_t yaw_limit = 2800 + abs(g.rc_4.control_in); + #else int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); - + #endif + // constrain output output = constrain(output, -yaw_limit, yaw_limit); @@ -405,18 +409,9 @@ get_nav_yaw_offset(int yaw_input, int reset) return ahrs.yaw_sensor; }else{ - // re-define nav_yaw if we have stick input - if(yaw_input != 0){ - // set nav_yaw + or - the current location - _yaw = yaw_input + ahrs.yaw_sensor; - // we need to wrap our value so we can be 0 to 360 (*100) - return wrap_360(_yaw); - - }else{ - // no stick input, lets not change nav_yaw - return nav_yaw; + _yaw = nav_yaw + (yaw_input / 50); + return wrap_360(_yaw); } - } } static int16_t get_angle_boost(int16_t value)