From 44b16b7b610230a93db588e510f382aa8009221d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jul 2012 12:26:13 -0700 Subject: [PATCH] Arducopter: Temp reversion to old Yaw controller. --- ArduCopter/Attitude.pde | 35 ++++++++++++++++++++--------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 9292a358b8..4fc0106ce6 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -129,6 +129,13 @@ get_acro_pitch(int32_t target_rate) static int16_t get_acro_yaw(int32_t target_rate) +{ + target_rate = g.pi_stabilize_yaw.get_p(target_rate); + return get_rate_yaw(target_rate); +} + +static int16_t +get_acro_yaw2(int32_t target_rate) { static int32_t last_target_rate = 0; // last iteration's target rate int32_t p,i,d; // used to capture pid values for logging @@ -140,36 +147,36 @@ get_acro_yaw(int32_t target_rate) target_rate = g.pi_stabilize_yaw.get_p(target_rate); current_rate = omega.z * DEGX100; rate_error = target_rate - current_rate; - + //Gain Scheduling: //If the yaw input is to the right, but stick is moving to the middle - //and actual rate is greater than the target rate then we are + //and actual rate is greater than the target rate then we are //going to overshoot the yaw target to the left side, so we should - //strengthen the yaw output to slow down the yaw! - -#if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME) + //strengthen the yaw output to slow down the yaw! + +#if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME) if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){ decel_boost = 1; } else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){ decel_boost = 1; } else if (target_rate == 0 && abs(current_rate) > 1000){ - decel_boost = 1; + decel_boost = 1; } else { decel_boost = 0; } - + last_target_rate = target_rate; - + #else decel_boost = 0; - + #endif - + // separately calculate p, i, d values for logging // we will use d=0, and hold i at it's last value // since manual inputs are never steady state - + p = g.pid_rate_yaw.get_p(rate_error); i = g.pid_rate_yaw.get_integrator(); d = 0; @@ -177,10 +184,10 @@ get_acro_yaw(int32_t target_rate) if (decel_boost){ p *= 2; } - + output = p+i+d; - // output control: + // output control: // constrain output output = constrain(output, -4500, 4500); @@ -197,8 +204,6 @@ get_acro_yaw(int32_t target_rate) #endif return output; - - } static int16_t